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Abstract 

A  multibody  car  system  is  a  non-nilpoientf  non-regular,  iriangularizahle  and  well-controllable 
system.  One  goal  of  the  current  paper  is  to  prove  this  obscure  assertion.  But  its  main  goal  is 
to  explain  and  enlighten  what  it  means. 

Motion  planning  is  an  already  old  and  classical  problem  in  Robotics.  A  few  years  ago  a  new 
instance  of  this  problem  has  appeared  in  the  literature  i  tnoiion  planning  for  nonholonomic 
systems.  While  useful  tools  in  motion  planning  come  from  Computer  Science  and  Mathematics 
(Computational  Geometry,  Real  Algebraic  Geometry),  nonholonomic  motion  planning  needs 
some  Control  Theory  and  more  Mathematics  (Differential  Geometry). 

First  of  all,  this  paper  tries  to  give  a  computational  reading  of  the  tools  from  Differential 
Geometric  Control  Theory  required  by  planning.  Then  it  shows  that  the  presence  of  obstacles 
in  the  real  world  of  a  real  robot  challenges  Mathematics  with  some  difficult  questions  which 
are  topological  in  nature,  and  have  been  solved  only  recently,  within  the  framework  of  Sub- 
Riemannian  Geometry. 

This  presentation  is  based  upon  a  reading  of  works  recently  developed  by  Murray  and  Sastry 
[39],  Lafferiere  and  Sussmann  [55],  and  Bellaiche,  Jacobs  and  Laumond  [5]  [33]. 
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1  Introduction 


The  motion  planning  problem  (MP)  is  certainly  one  of  the  best  formulated  ones  in  Robotics.  It 
raises  two  questions  : 

•  Can  a  robot  reach  a  given  goal  while  avoiding  the  obstacles  of  its  environment  ?  This  is  the 
decision  problem. 

•  If  the  answer  to  the  previous  question  is  yes,  what  path  may  it  follow  ?  This  is  the  complete 
problem. 

The  geometric  formulation  of  this  problem  is  known  as  the  piano  mover  problem.  This  formu¬ 
lation  considers  the  motion  of  rigid  bodies  amidst  obstacles  in  the  3-dimensional  Euclidean  space. 
The  placement  (translation  and  rotation)  of  a  body  in  the  Euclidean  space  is  given  by  a  point  in 
a  6-dimensional  space.  Some  geometric  relationship  between  the  bodies  may  appear  for  a  given 
robotic  system  (as  is  typical  for  a  robot  arm).  They  are  translated  into  equations  between  the 
placement  parameters  of  the  bodies.  These  are  called  the  holonomic  links.  They  restrict  the  space 
of  the  allowed  placements  to  a  subspace  of  the  placement  spaces  of  all  the  bodies.  This  subspace 
is  called  the  configuration  space.  Finally,  a  configuration  of  the  robot  is  represented  by  a  point  of 
the  configuration  space  that  defines  precisely  the  domain  occupied  by  the  robot  in  Euclidean  space. 
A  point-path  in  the  configuration  space  corresponds  to  a  motion  of  the  robot.  For  a  holonomic 
system,  we  have  as  many  degrees  of  freedom  as  is  needed  to  follow  any  path. 

Therefore,  for  holonomic  systems,  the  existence  of  a  collision-free  trajectory  is  characterized  by 
the  existence  of  a  connected  component  in  the  admissible  (i.e.,  collision-free)  configuration  space.  To 
solve  MP,  it  is  enough  to  compute  the  admissible  configuration  space  (i.e.,  transform  the  obstacles 
in  the  Euclidean  space  into  “obstacles”  in  the  configuration  space),  and  then  explore  its  connected 
components. 

Since  the  seventies  this  problem  has  attracted  many  researchers  working  in  Robotics  and  beyond, 
in  Computational  Geometry  and  Algebraic  Geometry.  See  [48]  for  a  recent  overview,  [60]  for  a 
review  of  the  various  approaches  of  the  problem,  and  [27]  as  the  first  genuine  book  on  the  subject. 

However,  there  are  cases  in  which  this  formulation  of  motion  planning  is  not  sufficient.  In  the 
last  four  years,  an  example  of  the  limitation  of  the  piano  mover  formulation  has  been  investigated  : 
planning  constrained  motions  where  constraints  are  nonholonomic  in  nature.  A  nonholonomic  link 
is  expressed  as  a  non-integrable  equation  involving  derivatives  of  the  configuration  parameters. 
Such  constraints  are  expressed  in  the  tangent  space  at  each  configuration;  they  define  the  allowable 
velocities  of  the  system,  and  they  cannot  be  eliminated  by  defining  a  more  restricted  configuration 
space  manifold.  Thus,  the  main  consequence  of  a  nonholonomic  constraint  is  that  an  arbitrary 
path  in  the  admissible  configuration  space  does  not  necessarily  correspond  to  a  feasible  trajectory 
for  the  robot.  Therefore,  the  existence  of  a  collision-free  trajectory  is  not  a  priori  characterized  by 
the  existence  of  a  connected  component  in  the  admissible  configuration  space. 

Planmng  motions  for  nonholonomic  systems  is  not  as  new  in  other  communities  as  in  the 
community  working  on  obstacle  avoidance  for  robots^.  This  problem  is  well  known  in  Nonlinear 

‘Notice  that  nonholonomic  motion  planning  appears  also  in  some  spatial  applications,  for  systems  (like  space- 
stations  or  satellite)  using  internal  motion  and  submitted  to  conservation  laws  (see  [40]  [42]). 
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Control  and  in  Differential  Geometry.  Important  results  have  been  obtained  over  the  last  two 
decades,  while  the  first  results  seem  dated  from  the  thirties  with  Chow’s  work  [12]. 

Notwithstanding,  Robotics  brings  to  the  front  an  important  constraint  which  is  not  usually  taken 
into  consideration  :  the  planner  has  to  produce  trajectories  that  avoid  the  obstacles.  Moreover,  by 
its  applications  to  the  real  world,  it  reqtiires  effective  and  efficient  computational  tools,  rather  than 
just  proofs  of  existence. 

Results  useful  to  our  problem  can  be  found  in  publications — often  difficult  ones — from  other 
communities  than  the  Robotics  one.  Because  the  viewpoints  are  different,  they  attack  only  some 
aspects  of  the  problem,  and  use  different  terminologies.  The  goal  of  this  paper  is  to  enlighten 
these  points  of  view  by  a  computational  one  (the  right  point  of  view  for  the  planning  problem) 
and  to  stress  the  connections  between  motion  planning  and  differential  geometric  control  theory. 
This  study  has  to  be  viewed  as  an  informal  statement  of  these  connections,  hopefully  readable 
by  a  non-specialist  in  control  theory  or  in  differential  geometry  (as  the  author  is).  A  spotlight 
will  be  focused  on  some  concepts  from  these  theories,  using  a  minimal  formalism  while  trying  to 
understand  where  and  why  these  are  pertinent  as  far  as  the  motion  planning  problem  is  concerned. 
It  is  clear  that  an  in-depth  study  of  these  concepts  needs  the  precision  given  by  the  mathematical 
formalism  :  for  each  concept  there  will  be  references  introducing  or  using  it. 

Along  this  study  we  take  a  multibody  car  system  (i.e.,  like  a  luggage  carrier  in  an  airport)  as 
an  example  of  application.  The  results  (Section  3  and  Section  5)  for  this  example  constitute  a 
contribution  by  themselves.  They  can  be  read  independently  of  the  rest. 

The  decision  problem  of  planning  for  nonholonomic  systems  is  related  to  their  controllability^  : 
more  precisely  the  existence  of  a  coUision-free  trajectory  for  a  controllable  nonholonomic  robot 
is  characterized  by  the  existence  of  an  open  connected  component  of  the  admissible  configuration 
space.  The  decision  problem  is  then  similar  to  that  of  the  piano  mover  problem.  This  result 
constitutes  the  first  main  link;  it  has  been  studied  simultaneously  in  several  research  groups  :  [31] 
[34]  [3]  [38].  It  is  based  upon  the  Lie  algebra  rank  condition,  and  will  be  recalled  in  Section  2. 

Section  3  presents  a  three-step  modeling  of  the  multibody  cax  system,  from  a  purely  geometric 
model  leading  to  the  definition  of  the  configuration  space  (Section  3.1),  to  four  distinct  control 
models,  all  corresponding  to  practical  applications  (Section  3.3),  via  a  differential  model  (Section 
3.2).  The  differential  model  finally  enables  us  to  give  a  unified  proof  of  controllability  encompassing 
all  the  envisaged  systems  (Section  5). 

Section  4  refines  the  presentation  of  Section  2,  by  considering  the  computational  aspects  of  the 
problem  :  is  a  system  controllable  ?  There  is  a  semi-deddable  procedure  for  this  problem,  which  is 
supported  by  several  concepts  of  differential  geometric  control  theory.  Then  this  paper  introduces 
the  well-controllability  notion,  in  relation  to  the  planning  problem. 

Nevertheless,  at  this  stage  the  complete  problem  of  nonholonomic  motion  planning  remains 
unsolved. 

*The  use  of  the  term  “controllability”  in  this  context  is  fuzzy  in  the  community.  Indeed,  the  meaning  we  use  here 
is  related  to  the  reachability  concept.  A  nonholonomic  system  may  be  controUable  by  open  loops.  It  does  not  have  to 
be  controllable  by  closed  loops  (see  [47]  for  a  study  of  feedback  controls  for  a  nonholonomic  wheeled  cart).  It  would 
be  better  to  use  the  notion  of  a  completely  nonholonomic  system  related  to  the  concept  of  a  distribution  (see  [58]). 
This  paper  adheres  to  Sussmann’s  terminology  [54],  which  seems  to  have  reached  some  state  of  general  acceptance 
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Section  6  considers  the  complete  problem.  We  will  see  that  the  key  question  is  topological  in 
nature.  While  motion  planners  for  nonholonomic  systems  have  blossomed  through  the  last  two 
years  (see  Appendix  1),  very  recent  contributions  pursue  a  deep  study  of  the  differential  geomet¬ 
ric  tools  available  for  solving  the  complete  problem.  [33]  presents  an  efficient  planner  for  mobile 
robots  and  shows  that  its  strategy  can  be  generalized.  [55]  presents  a  general  planner  based  upon 
a  general  constructive  proof  of  controllability.  These  results  let  us  stress  the  main  difficulty  for 
bmlding  efficient  planners  :  while  obstacle  avoidance  requires  us  to  consider  the  “natural”  Rieman- 
nian  topology  of  the  configuration  space  (i.e.,  induced  by  the  natural  Hausdorff  metric  working 
in  the  robot  environment),  the  trajectories  allowed  by  the  nonholonomic  constraints  compel  us 
to  consider  another  topology  in  this  space  :  the  topology  induced  by  the  length  of  the  shortest 
allowed  path  between  two  points.  Such  a  metric  is  known  as  a  sub-Riemannian  (or  singular,  or 
Carnot-Caratheodory)  metric.  In  fact,  using  sub-Riemannian  geometry  (see  [7]  [51]  [58]  [36]),  it 
is  possible  to  show  that  both  topologies  are  the  same.  This  result  enables  us  to  conclude  on  the 
generality  of  the  approaches  presented  in  [33]  and  [55].  Nevertheless,  because  we  are  interested  in 
the  computational  point  of  view,  we  have  to  study  more  deeply  the  shape  of  the  sub-Riemannian 
metrics  in  order  to  estimate  the  combinatorial  complexity  of  the  planners.  This  study  has  been 
done  m  [33]  for  the  case  of  the  car-like  robot.  This  section  points  at  the  reference  [58]  that  gives 
precisely  the  general  and  finest  form  of  the  sub-Riemannian  metric  we  need  to  conclude  on  the 
complexity  of  the  complete  problem. 

Appendix  1  overviews  other  results  related  to  nonholonomic  motion  planning  :  they  do  not 
use  intensively  the  tools  we  present  in  this  paper,  but  they  are  interesting  nonetheless  from  either 
a  theoretical  or  a  practical  point  of  view.  Following  appendices  give  the  various  computations 
for  the  examples  presented  along  the  paper  (Appendix  2),  together  with  the  tedious  proof  of  the 
controllability  result  presented  in  Section  5  (Appendix  3). 

Two  starting  points  are  at  the  origin  of  this  working  paper.  The  first  one  has  been  the  work  of 
Barraquand  and  Latombe  on  the  controllability  of  multibody  systems  (see  [4]).  This  report  yields 
a  proof  of  controllability  in  a  general  case  involving  up  to  n  trailers.  The  second  point  has  been  a 
reading  of  papers  written  by  Murray  and  Sastry  [38]  [39]  and  by  Laiferiere  and  Sussmann  [55].  It 
seemed  interesting  to  clarify  the  relationship  between  these  approaches  of  the  planning  problem, 
taking  also  into  account  the  approach  ([30]  [33])  developed  within  the  Hilare  mobUe  robot  project 
[16]  [10]  by  Jacobs  and  the  author. 
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2  From  Planning  to  Control 


•  Until  a  very  recent  period,  the  main  contribution  to  nonholonomic  motion  planning  (independently 
developed  in  [31]  and  [34])  has  been  to  solve  the  decision  part  of  the  nonholonomic  motion  plan¬ 
ning  problem,  via  differential  geometry  and  control  theory.  See  [3]  for  a  clear  presentation  of  the 
necessary  tools  that  we  examine  here. 

While  the  constraints  due  to  the  obstacles  are  expressed  directly  in  the  manifold  of  configura¬ 
tions,  nonholonomic  constraints  deal  with  the  tangent  space.  In  the  presence  of  a  link  between  the 
robot’s  parameters  and  their  derivatives,  two  questions  arise  : 

•  Is  this  link  holonomic  ?  (i.e.,  does  it  reduce  the  dimension  of  the  configuration  space  ?) 

•  If  not,  does  it  reduce  the  accessible  configuration  space  ? 

In  the  case  of  r  links  corresponding  to  r  equations  linear  in  the  derivatives  of  the  n  parameters, 
these  equations  determine  what  is  called  an  (n— r)-distribution  A  on  the  manifold  of  configurations. 
The  answer  to  the  first  question  is  then  given  by  Frobenius’  theorem  (see  for  instance  [49]):  the 
equations  are  integrable  if  and  only  if  the  distribution  A  is  closed  under  the  Lie  bracket  operation. 
Let  us  recall  that  the  Lie  bracket  of  two  vector  fields  X  and  Y  is  defined  as  [X,  y]  =  dX.Y  —  dY.X. 
A  sample  of  computation  examples  appears  in  Appendix  2. 

From  a  control  theory  perspective,  a  control  is  a  function  which  allows  us  to  choose  the  system 
state  velocity  at  each  instant  by  a  careful  weighting  of  smooth  vector  fields.  The  control  Lie  algebra 
associated  with  A,  denoted  by  2>A(A),  is  the  smallest  distribution  which  contains  A  and  is  closed 
under  the  Lie  bracket  operation.  The  answer  to  the  second  question  is  then  given  by  the  non-linear 
system  controllability  theorem  (see  for  instance  [53]  [35]  [17]):  if  the  rank  of  the  Lie  algebra  is 
full  at  a  given  configuration  c,  then  there  exists  a  neighborhood  Af  of  c  whose  points  represent 
configurations  reachable  by  the  system  moving  from  c  along  an  admissible  path.  Moreover,  this 
path  stays  in  Af.  This  condition  is  known  as  the  “rank  condition”;  it  is  a  local  condition. 

If  the  rank  condition  holds  everywhere  in  the  configuration  space,  then  the  robot  is  termed 
controllable^.  From  our  planning  point  of  view,  the  main  consequence  is  that  the  existence  of  a 
collision-free  trajectory  is  characterized  by  the  existence  of  a  connected  component  in  the  free  (i.e., 
with  neither  collision  nor  contact)  configuration  space. 

Therefore,  apart  from  topological  subtleties  dealing  with  motions  in  contact,  the  decision 
problem  of  motion  planning  for  controllable  systems  is  the  same  as  for  holonomic 
ones^. 

The  difference  is  more  involved  with  the  complete  problem.  The  previous  result  answers  the 
question  of  the  existence  of  a  feasible  trajectory,  that  is,  the  decision  problem,  but  does  not  solve  the 

^In  fact  a  controllable  system  requires  only  that  the  rank  condition  holds  nearly  everywhere,  that  is  on  a  dense 
subset  of  the  manifold.  Various  precise  controllability  concepts  appear  involving  the  “size”  of  reachable  sets  and  the 
“size”  of  sets  where  the  rank  condition  holds  (see  [54]).  Omitting  them  does  not  affect  our  purpose. 

^Section  3  highlights  the  difficulty  for  proving  the  controllability  of  a  system.  Since  we  are  interested  in  the 
computational  point  of  view,  we  are  looking  for  a  procedure  that  allows  us  to  conclude.  As  we  will  see,  this  problem 
is  not  trivial. 
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problem  of  efficiently  producing  an  admissible  trajectory.  To  the  extent  of  the  author’s  knowledge, 
there  was  no  general  (instructive  proof  of  the  result  mentionned  above  until  the  recent  contribution 
of  Lafferiere  and  Sussmann  [55]®.  Their  proof  leads  to  the  design  of  a  general  nonholonomic  motion 
planner  in  the  absence  of  obstacles  (see  Section  6). 

At  this  stage  we  can  just  hope  that  the  search  for  a  solution  for  a  nonholonomic  system  can 
be  guided  by  a  collision-free  trajectory  for  the  associated  holonomic  system.  Indeed,  thanks  to  the 
local  property  above,  a  controllable  robot  can  be  steered  close  to  any  path  as  long  as  there  is  a 
“small  gap**  between  the  reference  path  and  the  obstacles.  This  idea  is  precisely  the  basis  of  the 
two  strategies  defined  in  [55]  and  [33]  that  we  will  study  in  Section  6.  It  appears  clearly  that  the 
key  question  for  developing  such  a  strategy  deals  with  the  size  of  the  “gap”,  i.e.,  with  the  topology 
induced  by  the  nonholonomic  constraints.  This  aspect  is  detailed  in  Section  6. 

Now  let  us  define  the  multibody  car  system. 


“Specilic  constructive  proofs  appear  in  [28]  for  the  car-like  robot  and  in  [32]  for  the  car-like  robot  pulling  a  trailer. 
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3  The  Modeling  of  a  Multibody  Car  System 


Figure  1  shows  what  we  call  a  multibody  car  system.  It  corresponds  to  a  car-like  robot  pulling 
and  pushing  trailers  (like  a  luggage  carrier  in  an  airport).  In  order  to  grasp  the  planning  point 
of  view,  we  will  build  along  three  modeling  levels  for  this  system  :  a  geometric  model  (Section 
3.1),  a  differential  model  (Section  3.2)  and  a  control  model  (Section  3.3)  respectively.  Then  a 
close  examination  shows  that  four  different  control  systems  correspond  to  the  same  differential 
model;  this  is  the  differential  model  that  we  will  use  to  solve  the  decision  part  of  the  planning 
problem  (Section  5). 


Figure  1:  A  multibody  car  system 


3.1  Geometric  Model 

Let  us  consider  a  multibody  system  {Bq^Bu  •  •  •  »5n)  constituted  by  a  car  Bq  and  n  trailers  Bi.  We 
take  the  midpoint  between  the  rear  wheels  as  the  reference  point  for  each  body;  its  coordinates  are 
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denoted  by  x.  and  y.  in  some  fixed  Cartesian  frame  of  the  plane;  the  angle  6>.-  is  taken  between  the 

mam  axis  of  5.  and  the  x-axis  of  the  frame.  The  space  of  possible  placements  of  the  n  +  1  bodies 
IS  then  3(n  +  l)-dimensional. 

In  order  to  form  a  convoy  (e.g.,  the  car  pulling  the  n  trailers  B.-,  each  hooked  up  to  the  next 
hke  for  a  luggage  carrier),  each  trailer  Bi  is  assumed  to  be  hooked  up  to  the  midpoint  between  the 
reM  wheels  of  the  preceding  body  This  means  that  the  system  is  submitted  to  2n  holonomic 

hnks  which  yield  the  following  equations  in  the  placement  space^  : 

Xi  —  x,_i  =  —  cos  0i, 

Vi  —  Vi-i  =  -  sin  di. 

The  configuration  space  of  the  convoy  is  a  submanifold  of  dimension  3(n  +  1)  -  2n  =  n  +  3  in  the 
placement  space  of  all  the  bodies.  A  possible  parameterization  of  this  submanifold  is  to  pick  out 
the  (n  +  3>tuple  (xq,  yo,  6q,  ,  »„)  belonging  to  x  For  the  sake  of  simplicity,  we 

wiU  ^t  X  -  xo,  y  =  yo,0  =  00  and  <pi  =  0i  -  so  that  <pi  is  the  angle  between  the  axes  of  Bi 
and  e,_i,  and  use  ix,y,0,(pi,  . . .  ,(p„)  as  an  alternative  parameterization. 


3.2  Differential  Model 


Each  body  is  rolling  on  the  ground  without  sliding;  thus  it  is  submitted  to  the  classical  nonholonomic 


X,-  sin  0i  —  yi  cos  0i  =  0. 


^mark  :  This  equation  is  obvious.  We  just  want  to  mention  the  Mowing  fact  :  it  has  been 
obtained  without  any  reference  to  a  control  system;  we  have  just  used  some  elementary  kinematic 
considerations  (i.e.,  a  moving  rigid  body  has  only  one  instantaneous  center  of  rotation). 

The  system  is  subject  to  n  +  1  such  constraints.  Since  the  number  of  degrees  of  freedom  of  a 
mechanical  system  is  defined  as  the  difference  between  the  dimension  of  its  configuration  space  and 
the  number  of  independent  nonholonomic  links,  the  number  of  degrees  of  freedom  of  our  convoy  is 
(n  +  3)  -  (n  +  1)  =  2  (obviously,  the  2  degrees  of  freedom  of  the  car. . . ). 

These  constraint  equations  are  expressed  in  the  tangent  space  of  the  placement  space  of  all  the 
bodies.  In  order  to  translate  them  into  the  tangent  space  of  the  configuration  space,  we  have  to  get 
rid  of  the  variables  x,-  and  y,  for  *  0  .  By  combining  these  n  +  1  equations  with  the  derivatives 

of  the  2n  holonomic  equations,  we  obtain  the  following  system  of  n  +  1  linear  equations  : 


x'o  sin  0i  —  jto  cos  0i  -Yi^j<^os{0j-0i)  =  O. 
3=1 


A  configuration  c  being  given,  this  system  defines  a  plane  in  the  tangent  space  at  this  point;  this 
means  that,  for  a  possible  motion  passing  through  c,  the  velocity  vector  at  c— whenever  defined— 
has  to  lie  in  that  plane^.  If  we  compute  the  solution  of  the  resulting  system,  we  obtain  : 


‘The  hooking  system  is  important.  The  equations  do  not  simplify  if  the  trailers  are  hooked  behind  the  rear  axlei 
Moreover  the  problem  is  unsolved  for  this  generalization. 

resulte*  tW®  does  not  affect  the  generality  of  the  foUowinj 


•The  set  of  all  these  planes  has  the  structure  of  what  is  called  a  distribution  on  the  manifold. 
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4 


a  cos  6q 
a  sin  0Q 
/? 

a  sin(di  —  0o) 
acos(^i  -  ^o)sin(^2  —  ^i) 

n— 1 

a8in(0„  -  0n-i)  JJ  co8(tf,-  -  0i_i) 

«=i 

with  a  and  /?  any  two  reals.  Setting  (a,/?)  =  (1,0)  and  (a,/?)  =  (0, 1)  yields  a  basis  of  the  plane, 
namely  : 

Xa  =  (cos do,  sin 00,  0,  sin(di  —  do),  cos(di  -  do)  sin(d2  -  0i),  ... 

n— 1 

. . .  ,  sin(d„  -  d„_i)  JJ  cos(dj  -  dj_i)), 
i=i 

Xb  =  (o,  0,  1,  0,  ...  ,o). 

If  we  use  the  change  of  variable  (pi  =  d,-  —  d,_i,  the  same  computation  yields®  : 


Xo  = 
Vo  = 
do  = 
d,  = 
02  = 


e'n  = 


X  =  a  cos  d 
y  =  asind 
d  =  13 

(fii  =  —  a  sin  (pi 

tp2  =  ot  (sin  (fi  —  cos  (pi  sin  ^2) 

93  =  a  cos  y>i(sin  ^>2  —  cos  <p2  sin  ^>3) 


n— 2 


(fin  =  a  (sin  V>„_1  -  cos  y)„_i  sin  v>r> )  ]][ 

«=i 

as  the  parameterized  equations  of  the  plane,  and 

Xa  =  (cos  d,  sin  d,  0,  —  sin  (pi ,  sin  <pi  —  cos  ipi  sin  ^2,  •  •  • 

n— 2 

. . .  ,  (sin  (pn-l  -  cos  (pn-i  Sin  <Pn)  n  ^3)  ’ 


i=i 


Xb  =  (0,  0,  1,  1,  0,  ...  ,0) 


*  Unfortunately  the  general  shape  appears  only  from  the  6th  coordinate  on. 


11 


as  a  distinguished  basis. 

Recall  that  all  this  system  modeling  has  been  done  without  reference  to  any  control  system. 
It  has  been  built  just  from  the  non-sliding  hypothesis  applied  to  each  body  of  the  convoy.  So  any 
control  system  will  apply  to  the  framework  above.  We  will  use  the  generality  of  the  differential 
model  in  order  to  prove  the  controllability  of  the  convoy  for  several  control  systems. 

3.3  Control  Models 

At  this  moment,  we  have  not  mentioned  yet  how  the  system  moves.  We  have  just  suggested  that 
the  first  body  drives  the  convoy.  But  we  can  imagine  that  the  car  is  in  the  middle  of  the  convoy; 
better  it  may  be  possible  that  there  is  no  car,  and  just  some  effectors  controlling  the  angular 
joints  between  the  bodies  (as  in  some  japanese  snake-robots).  We  examine  here  four  examples  of 
control  systems  and  prove  that,  in  all  cases,  the  proof  of  controUability  of  these  systems  amounts 
to  computing  exactly  the  same  Lie  algebra,  namely  the  Lie  algebra  spanned  by  the  vector  fields  Xa 
and  Xj,  introduced  above. 


3.3.1  The  Convoy  is  Driven  by  Gofer 

The  Stanford  mobile  robot  Gofer  [11]  is  very  interesting  for  our  purpose.  Indeed  it  leads  to  intro¬ 
ducing  a  distinction  between  the  control  variables  and  the  parameters  which  is  pertinent  from  the 
point  of  view  of  the  planning.  Gofer’s  locomotion  system  is  clever  :  three  parallel  driving  wheels 
are  linked  together  by  some  mechanical  system;  their  velocities  are  controlled  by  a  same  motor 
(which  produces  forward  or  backward  motions),  while  a  second  motor  can  control  their  directions. 
Such  a  design  requires  that  there  be  a  part  of  the  vehicle  whose  direction  remains  fixed. 

H  the  non-rotating  part  has  a  geometric  shape  whose  projection  on  the  plane  contains  the 
projection  of  the  whole  robot,  then  the  robot  appears,  from  the  planning  point  of  view,  as  a 
translating,  but  non-rotating  body.  In  this  case  the  reference  frame  of  the  robot  is  linked  to  the 
fixed  part  and  the  configuration  space  is  only  2-dimensional. 

Otherwise,  if  the  projection  of  the  non-rotating  part  is  included  in  the  projection  of  the  other 
parts,  the  robot  appears  (always  from  the  planning  point  of  view)  as  a  2-dimensional  translating 
and  rotating  body.  The  configuration  space  is  thus  3-dimensional.  The  robot  looks  closely  like 
what  is  called  a  unicycle  :  this  means  that  the  speed  of  the  vehicle  and  its  direction  are  directly 
controlled  by  two  independent  motors^®. 

Therefore,  with  (x,  y,  0)  designating  a  configuration  of  the  robot,  the  control  system  is  : 

‘®The  distinction  we  have  introduced  is  meaningless  for  the  true  robot,  since  Gofer  is  circular  !  However,  this  is 
an  example  of  the  subtle  links  that  may  appear  between  a  geometric  model  for  planning  and  a  differential  mUel  for 
control :  think  of  the  case  where  none  of  the  projected  parts  would  be  included  in  the  other;  in  that  case  the  planning 
problem  would  concern  a  translating,  rotating  and  deformable  2-dimen8ional  body.  As  a  matter  of  fact.  Gofer  is  but  a 
pretext  to  introduce  the  interesting  case  of  the  unicycle. . .  And,  in  order  to  be  exhaustive  with  the  Stanford  Robotics 
Laboratory,  there  is  also  a  robot  called  Mobi  whose  sophisticated  locomotion  system  endows  with  the  privilege  of 
being  one  of  the  few  existing  holonomic  mobile  robots. 
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i 

y 

6 


cosO 

sin^ 

0 


Ml  + 


il 


tt2. 


Thus  if  we  attach  n  trailers  at  Gofer’s  rear  its  control  system  expresses  directly  as  the  two  vector 
fields  Xa  and  Xh  defined  in  the  preceding  section,  so  that  the  controllability  of  the  system  will  be 
established  by  computing  the  Lie  algebra  spanned  by 


3.3.2  The  Convoy  is  Driven  by  Hilare 

Consider  now  the  Hilare  family,  dwelling  at  Laas  [16]  [10].  The  three  mobile  robots  have  the  same 
classical  locomotion  system  ;  two  parallel  driving  wheels,  the  acceleration  of  both  being  controlled 
by  two  independent  motors.  Assume  that  the  distance  between  the  wheels  is  2,  that  the  reference 
point  of  the  robot  is  the  midpoint  of  the  wheels  and  that  the  main  direction  of  the  vehicle  is  the 
direction  of  the  wheels.  With  and  V2  as  their  respective  speeds,  the  control  system  is  : 


X  \ 

f  (vi  +  t;2)cosd  > 

0  N 

/  0  \ 

y 

(vi  +  V2)8ill^ 

0 

0 

9 

= 

Ul  -  V2 

+ 

0 

Ml  + 

0 

Vi 

0 

1 

0 

\V2  ) 

0  j 

\0) 

1  / 

In  fact,  from  the  planning  point  of  view  that  we  take  in  this  paper  (the  speed  at  a  configuration  is 
not  considered  as  a  part  of  the  problem),  this  system  can  be  rewritten  in  the  following  way  : 


y 

e 


cosO  \ 
sinO 


Vi  + 


1  } 


COS0  \ 

sind  I  V2- 

-1  J 


If  we  add  the  trailers,  the  vector  fields  corresponding  to  the  controls  are  clearly  Ya  =  Xa  Xt 
and  Yb  =  Xa  —  Xb.  In  order  to  prove  the  controllability  of  the  convoy  we  have  to  compute  the 
dimension  of  the  Lie  algebra  spanned  by  {l^,yi}.  Due  to  the  bilinearity  of  the  Lie  bracket,  it  is 
equivalent  to  start  with  {Xa,Xb}. 


3.3.3  The  Convoy  is  Driven  by  a  Real  Car 

The  case  of  a  real  car  is  a  little  bit  more  complicated.  Indeed,  in  addition  to  the  nonholonomic  link, 
a  specific  constraint  appears  :  the  turning  radius  is  lower  bounded  (we  will  see  that  this  constraint 
does  not  cause  any  trouble). 

From  the  driver  point  of  view,  a  car  has  two  degrees  of  freedom  :  the  accelerator  and  the  steering 
wheel  (the  brake  is  a  kind  of  reverse  accelerator  and  the  clutch  pedal  is  reserved  to  European 
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drivers. . . ).  Take  the  midpoint  of  the  rear  wheels  as  the  reference  point.  Assuming  that  the 
distance  between  the  rear  and  the  front  axle  is  1,  we  denote  by  v  the  speed  of  the  car  and  by  <p  the 
angle  between  the  front  wheels  and  the  main  direction  of  the  car  Simple  arguments  show  that 
the  control  system  is  the  following  ; 


X  \ 

^  V  cos  ^  cos  9  ^ 

0 

/«\ 

y 

V  cos  <p  sin  e 

0 

0 

e 

vsiiKp 

+ 

0 

«1  + 

0 

V 

0 

1 

0 

\<p  J 

0  y 

u/ 

Again  we  are  only  interested  in  the  geometric  planning  problem.  Then,  since  the  position  of  the 
front  wheels  is  not  relevant  to  the  problem,  as  well  as  the  speed  of  the  vehicle,  the  system  simplifies 
into  : 

/i  \  I  cose  \  /  0  \ 

I  y  1  =  1  I  ®cosy>+  I  0  I  vsinv?- 

W/  V  0  y  vW 

This  is  a  non-linear  system  whose  controls  are  v  and  <p.  Anyway,  as  shown  by  the  form  of  the 
equation,  if  we  add  trailers,  the  controllability  of  the  convoy  will  still  be  proven  by  a  close  study  of 
the  Lie  algebra  spanned  by  Xa  and  Xi,. 

Remark  s  We  did  not  take  the  constraint  on  the  turning  radius  into  account.  This  constraint 
can  be  expressed  as  \ip\  <  y>o.where  ipo  is  a  strictly  positive  real.  From  the  point  of  view  of 
the  complete  control  model,  this  constraint  has  exactly  the  same  meaning  as  an  obstacle  in  the 
environment  (an  obstacle  in  the  environment  constrains  the  variables  x,  y  and  6  to  lie  in  some 
domain)  5  therefore,  it  does  not  affect  the  controllability  of  the  complete  system  and,  consequently, 
it  does  not  affect  the  controllability  of  the  simplified  system  either^^. 

3.3.4  The  Convoy  is  Oddly  Driven 

The  following  system  is  no  longer  classical.  Unlike  the  previous  ones  the  driver  system  does  not 
involve  the  first  body  alone  but  also  the  second  one.  These  bodies  being  articulated,  a  configuration 
will  be  denoted  by  (x,  y,  6,  <p).  The  specificity  of  the  system  lies  in  its  controls  :  the  linear  speed 
of  the  first  body,  and  the  angle  ip  between  the  bodies.  This  system  actually  corresponds  to  some 
vehicles  used  in  dvil  engineering*^. 

"More  ptedsely,  the  front  wheels  being  not  exactly  parallel  (else  they  would  slide),  we  take  the  average  of  their 
angles  as  the  tnm  angle. 

'^Baxraquand  and  Latombe  [4]  elaborate  on  this  point  and  take  advantage  of  it  to  exhibit  a  motion  planner  that  can 
even  manage  constraints  of  the  form  0  <  <pi  <ip<ip2  (*•«•!  the  car  can  only  turn  right  !);  independently  Bellaiche, 
Jacobs  and  Lanmond  give  a  proof  in  [5]  for  a  Hilare-like  system  verifying  f»i|  <  |t)2|  ,e.g.,  equivalent  to  a  car-like 
robot:  the  vehicle  turns  (|»2|  >  0)  only  if  its  linear  speed  is  not  zero  (|t)i|  >  0). 

Hnrteau  from  Polytechnical  Scholl  of  Montreal  mentioned  this  example  to  the  author:  that  kind  of  vehicle  is 
used  in  the  conveyance  of  ore  in  some  Canadian  salt  mines. 
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Let  ttj  be  the  speed  control  of  the  first  body;  let  be  the  control  of  the  angular  joint  of  the 
bodies.  Since  both  are  independent,  uj  is  applied  onto  a  vector  field  Ya  =  (cos  9,  sin  9,  a,  0)  while  «2 
is  applied  onto  a  vector  field  Yb  =  (0, 0, 6, 1).  A  close  study  of  the  differential  model  above  (section 
3.2)  leads  us  to  find  a  =  sin  ^  and  6=1.  The  control  system  is  : 


Therefore  the  system  is  controllable  if  the  Lie  algebra  spanned  by  Ya  and  Yb  is  four-dimensional. 
Furthermore,  Yb  is  equal  to  the  vector  field  Xb  instantiated  with  only  one  trailer.  Moreover  : 


=  Ao  +  sin  (pXb- 


Hence,  Ya  ends  up  to  be  a  linear  combination  of  Xa  and  Xb.  The  Lie  algebra  spanned  by  Ya  and  Yb 
is  the  same  as  the  Lie  algebra  spanned  by  Xa  and  Xb-  These  results  obviously  hold  if  we  add  trailers 
to  this  strange  vehicle  and  the  controllability  of  the  convoy  remains  subject  to  the  computation  of 
the  Lie  algebra  spanned  by  Xa  and  Xb  as  in  the  previous  cases. 

Conclusion 

The  decision  part  of  the  planning  problem  for  the  four  multibody  systems  above  is  the  same.  We  just 
have  to  study  the  distribution  appearing  in  the  differential  model.  This  study  appears  in  Section  5. 
We  now  discuss  some  computational  aspects  of  the  general  tools  we  can  use  to  achieve  this. 
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4  From  Control  to  Planning  :  a  Computational  Point  of  View 

4.1  What  is  the  Problem  ? 

Proving  the  controllability  of  an  n-dimensional  system  using  the  rank  condition  involves  showing 
that,  for  any  point  c  in  the  manifold,  there  exists  a  family  of  n  vectors  fields  in  the  Control  Lie 

Algebra  that  spans  R”  when  applied  to  c.  This  stirs  two  difficulties  due  to  the  local  and  global 
characteristics  of  the  problem  : 

•  At  any  specific  point  c,  finding  such  a  family  enables  us  to  conclude.  Not  being  able  to  find 
a  smtable  family  does  not  imply  that  there  is  none.  An  exhaustive  enumeration  of  possible 
families  is  impossible  since  there  is  an  infinity  of  potential  choices.  We  will  see  (Section  4.2) 
that  this  number  can  be  reduced  to  a  countable  one,  but  not  further,  leading  to  the  design  of 
some  semi-decidable  procedures.  We  will  proceed  then  to  giving  an  estimate  of  the  complexity 
of  procedures  testing  the  controllability  of  a  system  at  a  point. 

•  One  may  succeed  in  finding  bases  that  work  somewhere,  but  not  necessarily  everywhere.  There 
may  be  some  singularities.  An  interesting  problem  is  to  know  whether  such  singularities  have 
an  intrinsic  nature,  or  depend  upon  the  choice  we  make  (Sections  4.4  and  4.6). 

As  far  as  we  know,  testing  controllability  is  not  a  decidable  problem.  Nevertheless,  the  procedure 
we  define  always  terminates  provided  we  don’t  encounter  any  singularities  (Sections  4.7  and  4.8). 

The  material  of  this  section  uses  the  concepts  of  a  distribution,  also  known  as  a  Pfaffian  system 
(see  for  instance  [58]),  and  of  the  Free  Lie  Algebra  (see  [6]). 

Let  us  recall  that  every  Lie  operator  has  to  verify  skew-symmetry  \X,Y]  =  -\Y  X]  and  the 
J acobi  identity  [X,  [Y,  Z]]  -h  [T,  [Z,  X]]  +  [Z,  [X,  P]]  =  0. 

Consider  the  (n  -  r)-distribution  A  associated  with  a  robotic  system.  We  want  to  define  an 
algorithm  for  testing  the  controllability  of  that  system  at  a  point.  Precisely,  we  are  interested  in 
the  rank  of  LA(A)  (i.e.,  the  distribution  spanned  by  all  the  combinations  of  Lie  brackets  of  vector 

fields  in  A).  We  can  consider  a  basis  X  of  A  together  with  all  the  combinations  of  Lie  brackets 
built  upon  that  basis. 

To  do  this,  one  may  consider  a  brute  force  strategy  consisting  m  building  iteratively  the  following 
increasing  sequence  of  distributions  :  A,-  =  A._i  -H  [A._,,  A._i]  where  [Ai_i,  A._i]  is  the  linear 
space  spanned  by  all  the  brackets  [X,Y]  for  X  and  P  in  A,_i.  By  putting  A  =  Ai,  the  Control 
Lie  Algebra  LA(A)  is  precisely  defined  as  (J.-  A,-.  But  in  fact,  a  more  efficient  strategy  can  be  used. 
First  of  all,  let  us  define  a  parameter  estimating  the  complexity  of  a  combination  of  Lie  brackets. 
The  degree  of  a  combination  is  the  number  of  elements  in  X  defining  the  combination.  For  example 
the  degree  of  [•»[•»[•>  •]]>[•>[•>  •]]]  is  7.  Now,  our  strategy  will  consist  in  building  all  the  brackets  of 
a  given  degree,  step  by  step.  This  strategy  is  founded  on  the  following  iterative  construction.  We 
denote  A  by  Aj.  Then  A*  is  defined  by  : 


A«-A,_i+  ^  [Aj,AA,]. 

i+k=i 
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It  verifies  : 


Ai  C  A2  C  A3  C  •  •  •  C  Xi4(A)  and  LA(A)  =  At. 

i 

The  set  of  all  the  AtS  is  called  a  filtration  associated  with  A. 

Remark  :  Such  a  construction  can  be  viewed  as  a  “breadth-first”  construction.  Some  authors 
[58]  [57]  use  another  construction.  A  is  denoted  by  Ai.  Then  A,-  is  defined  by  : 

Ai  =  Ai_i  -1-  [Ai,  Ai_i]. 

Again  : 

Ai  C  A2  C  A3  C  •  •  •  C  LA{A). 

Such  a  construction  can  be  viewed  as  a  “depth-first”  construction.  Using  skew-symmetry  and  the 
Jacobi  identity,  we  may  verify  that  both  constructions  are  the  same^^.  We  will  prefer  the  first 
presentation  that  corresponds  exactly  to  the  concept  of  Phillip  HaU  families  introduced  below. 

On  the  other  hand,  at  a  point  c  of  our  manifold  (the  configuration  space),  (n— r)  <  rankAi(c)  < 
n  Moreover,  if  Ai(c)  ^  Ai_i(c),  then  rank  Ai(c)  >  rank  Ai_i(c).  Hence,  if  we  consider  the 
construction  locally  (i.e.,  by  applying  the  distributions  at  a  point),  we  can  conclude  that  there  exists 
an  index  Pc  such  that  Aj,j_i(c)  ^  Apj(c)  =  Ap^+i(c)  =  •••.  The  construction  always  stabilizes. 
The  index  pc  is  the  degree  of  nonholonomy  of  the  system  at  c.  Therefore  a  system  is  controllable  at 
c  if  and  only  if  rank  Ap^  =  n  (if  pc  =  1  we  are  locally  in  the  holonomic  situation).  Notice  that,  from 
a  global  point  of  view,  this  stabilization  property  is  not  true,  since  the  degree  of  nonholonomy  may 
change  from  point  to  point.  A  close  analysis  of  possible  singularities  shows  that  this  degree  may  be 
arbitrarily  high  at  singular  points — even  when  we  start  with  a  regular  distribution,  the  filtration 
we  build  may  acquire  some  weird  singularities.  So,  the  degree  of  nonholonomy  may  be  unbounded 
when  c  varies. 

Remark  :  It  is  possible  to  define  a  global  degree  of  nonholonomy  of  a  nonholonomic  system, 
as  the  maximum  of  pointwise  degrees  of  nonholonomy.  There  are  no  obvious  applications  of  this 
notion.  Also,  keep  in  mind  that  this  global  degree  can  be  infinite,  though  it  will  stay  bounded  in 
the  particular  cases  we  consider. 

4.2  A  Controllability  Algorithm 

In  this  section  we  define  an  algorithm  for  testing  the  controllability  of  a  given  system  at  a  point  based 
upon  the  previous  construction.  We  have  to  use  a  basis  X  of  A.  According  to  that  construction, 
we  build  : 


Xi  =  X 

Xi  =  Xi.x  (J  [Xj,Xk] 

i+k=i 

^*For  example,  take  [[X,y],[X,[X,Z]]],  an  element  of  As: 

][X,  n  [X,  [X,  m  =  -[X,  [X,  [Y,  [X,  m + 1^.  ty,  [X,  [X,  zm + [X,  [Z,  [X,  [X,  y]]]]  -  [Z,  [X,  [x,  [x,  y]]]]. 

Hence,  it  belongs  to  As  too. 

^^We  denote  by  Aj(c)  the  linear  subspace  of  the  tangent  space  in  c,  obtained  by  applying  the  distribution  Ai  at  c. 
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where,  now,  [Xj,Xk]  is  no  longer  viewed  as  a  linear  space,  but  as  a  finite  family  of  brackets.  Each 

Xi  contains  of  course  a  basis  of  A,-.  Again,  we  can  define  the  union  £A(A')  of  all  theses  families 
and  we  have  : 

C  A2  C  As  C  •  •  •  C  CA{X) 

This  is  dearly  an  mfinite  family,  but,  during  the  real  process,  we  can  check  out  the  added  elements 
if  they  happen  to  be  linearly  dependent  on  the  previous  ones. 

Even  if  we  know  only  about  the  relations  pertinent  to  the  concept  of  a  Lie  Algebra,  we  can  take 
advantage  of  these  to  compute  only  relevant  elements  of  what  is  called  the  Fr^  Lie  Algebra. 


4.2.1  Phillip  Hall  Families 

In  this  section^®  the  elements  of  CA{X)  are  considered  as  formal  expressions  produced  by  the 
construction  above,  i.e.,  they  are  not  actually  evaluated  as  vector  fields  belonging  to  a  distribution 
From  this  point  of  view,  CA{X)  is  considered  as  a  Free  Lie  Algebra.  Our  current  problem  is  to 
enumerate  a  basis  of  this  algebra,  i.e.,  to  get  rid  of  redundant  elements  using  only  skew-symmetry 
and  the  Jacobi  identity.  Such  a  basis  can  be  found  via  a  Phillip  Hall  family. 

The  degree  of  an  dement  X  in  CA{X)  is  denoted  by  degree(A)  :  this  is  the  degree  of  the 
monomid  defimng  X  .  According  to  our  notations,  a  Phillip  Hall  family  (PH-family  for  short) 
of  LAIX),  is  any  totally  ordered  subset  (VH,  ■<)  such  that  : 

•  If  A  €  VH,  Y  G  VH  and  degree(X)  <  degree(F)  then  X  ■<  F; 

•  XCVH; 

•  VHnX2  =  {[X,Y],  X^Y); 

•  An  dement  X  G  CA{X)  with  degree(A)  >  3  bdongs  to  VH  if  and  only  if  A  =  fCf  fF  PFll 

with  If,  F,  IF  in  PH,  [F,  IF]  in  PH,  F  Cf  X  [F,  IF]  and  F IF. 

The  main  property  of  a  PH-family  is  that,  taking  skew-symmetry  and  the  Jacobi  identity  into 
account,  it  yields  a  basis  of  the  free  Lie  algebra  £A(X)  [6]. 

The  proof  of  existence  of  such  a  fanuly  is  easy;  it  is  an  iterative  one.  In  the  context  of  our 
control  problem,  it  can  be  extended  into  the  following  algorithm. 

‘‘The  material  used  in  this  section  comes  from  [6].  We  want  just  to  give  a  rough  idea  of  the  concept  and  of  its 
pertmence  with  tesp^  to  our  problem.  Interested  readers  will  find  a  more  rigorous  presentation  in  this  reference. 
Wte  use  the  word  degree”  with  two  different  meanings,  according  to  whether  we  speak  of  a  bracket  or  of  a 

nonholonoimc  system.  This  may  introduce  some  confusion,  but  both  terms  are  already  used  in  the  Kterature  (see  for 
instance  [57]).  ' 
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4.2.2  The  Algorithm 

The  idea  is  to  build  a  PH-family,  based  upon  a  graded  family  of  sets  W,,  where  7i,  is  a  part  of  Xi. 
We  will  also  build  a  total  order  •<  on  the  union  W,-.  Assume  first  that  X  is  totally  ordered  by  <  and 
set  Hi  =  X.  The  order  -<  on  Hi  is  the  same  as  the  order  <  on  X.  The  next  set  H2  is  defined  as 
the  set  of  all  [A,y],  with  X,Y  elements  of  Hi  and  X  -<  Y.  Endow  H2  with  any  total  order,  and 
define  -<  on  (JWj  by  setting  X  ~<Y,  for  X  in  Hi  and  Y  in  H2> 

The  rest  of  the  algorithm  consists  in  building  the  sets  Hi  iteratively.  Suppose  the  family 
Hi,H2,  ...  ,Hi-i  is  given.  Denote  it  as  H.  Define  H{  according  to  the  definition  of  a.  PH-family 
That  is,  Hi  is  the  set  of  all  X  =  [U,  [V,  W]]  verifying  :  U  €  Hj,  [V,  W]  €  Hi-j,  V-<U  ^[V,W]  and 
V  -<W.  Choose  a  total  order  on  Hi  and  extend  it  to  :  X  ^Y  if  X  eH  and  Y  6  Hi.  It  is 

almost  obvious  that  the  family  Hi  is  a  PH-family  and,  furthermore,  that  the  degree  of  an  element 
of  Hi  is  precisely  i. 

We  can  use  this  construction  to  design  an  algorithm  for  testing  the  controllability  of  a  system 
at  a  point  c  of  the  manifold.  Our  algorithm  adds  new  brackets  to  the  PH-family  step  by  step,  but 
now,  we  check  further  the  value  of  each  new  bracket  as  a  potential  member  of  a  basis  at  c.  If  we 
ultimately  obtain  a  basis,  the  system  is  controllable  at  c. 

In  the  following  procedure,  B  denotes  the  free  family  that  will  eventually  become  a  basis,  cnt  is  the 
current  number  of  element  of  that  basis.  The  initial  distribution  is  (n  —  r)-dimensional  at  the  point  c.  For 
an  order  on  H ,  we  assume  that  we  have  an  initial  order  on  X ;  then  we  simply  take  the  order  of  chronological 
computation.  Finally  [xj  is  the  integer  part  (floor  function)  of  the  real  x. 

Procedure  Controllability{c) 

(initialize  Hi) 

Hi  *-X 
B  *-X 
cnt  *—n  —  r 
(build  H2) 

For  X,Y  iaHuX  <Y  do 
add[X,y]in«2; 

If  {[X,  y](c)}  UB(c)  is  a  free  family 
then 

add  [X,y]  in  5 
cnt  *-  cnt  +  1 

i*-2 

While  cnt  <  n  do 
i  <-  t  +  1 

(build  Hi) 

For  1  <  i  <  l»72j  do 

For  X  €Hj,Y  =  [U,V]€  Hi-j  do 

If  :<  X  (2) 

then 

add  [X,y]inW< 

If  {[X,y](c)}UjB(c)  is  a  free  family  W 
then 

add  [X,y]  inB 
cnt  *—  cnt  -I- 1 
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One  CM  verify  that  the  procedure  builds  sets  Hi  defining  a  PH-family.  Therefore,  it  appears  clearly 
that  the  system  is  controllable  at  c  if  and  only  if  Controllahility{c)  terminates.  This  also  means 
that  the  procedure  never  stops  otherwise. 


of  ^  I  ^  =  {Xu  X2).  The  first  14  elements 

of  the  PH-fanuly  generated  by  the  procedure  (if  it  does  not  stop  before)  are: 


X2 


Xz  —  A4  —  [Ai,  [Ai, A2]] 

A6  =  [A2,[Ai,A2]] 


A6  =  [Ai,[Ai,[Ai,A2]]] 
A7  =  [A2,[Ai,[Ai,A2]]] 
^8  =  [A2,[A2,[Ai,A2]]] 


A9  =  [Ai,[A,,[Ai,[Ai,A2]]]] 
Aio  =  [A2,(Ai,[Ai,[Ai,X2]]]] 
Aii  =  [A2,[A2,[A,,[Ai,A2]]]] 
Ai2  =  [A2,[A2,[A2,[Ai,A2]]]] 
A,3  =  [[Ai,A2],[Ai,[Ai,A2]]] 
Ai4  =  [[Ai,A2],[A2,[Ai,A2]]] 


fl  IJ^^V*”***?  ^  :  Consider  now  the  controUability  aspect.  Replace  Aj  and  Aj  by  the  vector 

fields  Ao  and  A6  defimng  the  nonholonomic  distribution  of  the  2-trailer  convoy  (see  Section  3  2) 
The  configuration  space  is  a  5-dimensionaJ  manifold.  Let  c  be  a  point  of  coordinates  (*,  y,  6,  <pi, 

X  16X0.0  ■ 


^  cos  9  > 

/  0  \ 

sin^ 

0 

0 

A2  = 

1 

—  sin  <pi 

1 

sin  ipi  —  cos  (pi  sin  (p2  j 

lo/ 

The  first  elements  of  a  PH-family  are  displayed  in  Example  Part  1  (see  Appendix  2  for  the  coordi¬ 
nates  of  the  various  vector  fields).  We  can  verify  that  the  algorithm  stops  with  {Ai,  A2,  A3,  A4,  Ae) 

^  a  basis  for  every  point  c  verifying  (pi^^  mod  ir.  The  algorithm  stops  with  { Ai ,  A2,  A3,  A4  Ag) 
for  the  remaining  hyperplane^®.  ^ 

Remark  :  Finally,  the  rank  condition  holds  everywhere  and  we  can  conclude  that  the  corre¬ 
sponding  system  is  controllable. 


In  this  example,  notice  that  the  algorithm  checks  6-2  =  4  “candidates”  in  the  first  case,  and 
9^2  7  in  the  second  one.  What  happens  in  the  general  case  ? 


The  core  of  the  algorithm  is  the  construction  of  a  PH-family.  The  dimension  n  of  the  manifold 
being  a  constant  integer  of  our  problem,  the  only  tests  needing  a  subroutine  depending  on  n  are  (1) 
and  (3).  Their  complexity  is  asymptotically  ne^gible.  Therefore  the  worst-case  complexity  of  the 
algorithm  is  dominated  by  the  complexity  of  building  a  PH-family.  The  relevant  parameter  is  the 
value  oft  when  the  algorithm  stops.  Because  of  the  test  (2),  our  procedure  for  building  a  PH-family 
IS  not  optimal  .  But,  here,  we  just  want  to  find  the  minimal  complexity  of  any  algorithm  that 


V  1^*  7  Xr,A^i{X^,X,  X„X„X,)  =  -C06(^0.  Ar  = 
aet{Ai,  A2,A^3,  A4,  A#}  =  —1  —  co8'(^i)co8(^j). 


0,  Xt  =  —A3  and  finally 


®It  seems  possible  to  define  an  optimal  one.  Maybe  snch  a  procedure  afieady  exists  in  the  literature  ? 
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builds  a  PH-family.  Now,  the  complexity  of  computing  all  the  elements  of  a  set  Hi  is  bounded 
below  by  the  number  of  all  the  elements  in  Hj,  j  <  t,  and  it  has  been  proven  that  this  number  is 

«(*)  =  7 

d\i 

where  fi  designates  the  Mobius  function. 


:  N* 
m 


{-1,0,1} 

Mm)  =!"_,)* 


if  m  is  the  square  of  a  prime  integer 

otherwise,  where  k  is  the  number  of  primes  dividing  m. 


For  example,  setting  (n  -  r)  =  m  ,  we  have  a(l)  =  m,  a(2)  =  -  m),  a(3)  =  |(m^  -  m), 

“(4)  =  \{rn^  -  m*),  q(5)  =  i(m®  -  m),  a(6)  =  g(m®  -  +  m).  One  may  verify  the  first 

5  values  on  the  current  example. 

If  the  algorithm  runs  for  a  point  c  and  stops  with  a  family  Hi,  the  system  is  said  to  be  completely 
nonholonomic  at  c  (i.e.,  all  missing  dimensions  can  be  recovered;  its  degree  of  nonholonomy  is 
maximal;  it  is  controllable).  Besides,  its  degree  of  nonholonomy  at  c  is  i. 

We  have  to  prove  this  latter  result.  Indeed,  the  algorithm  above  clearly  depends  upon  the 
basis  A’  we  chose  for  the  distribution  A.  However,  the  concept  of  degree  of  nonholonomy  does  not. 
Now,  it  is  a  general  result  from  the  Lie  Algebra  Theory  that  Hj  constitutes  a  basis  of  the 
nilpotent  free  Lie  algebra  CAi{X)  defined  by  taking  all  the  brack^s  of  degrees  less  than  i  and  by 
kilhng  all  the  brackets  of  greater  degrees.  See  [6]  for  details.  Therefore,  t  does  not  depend  on  our 
choice  of  a  basis  X  of  A.  It  truly  is  the  degree  of  nonholonomy  that  has  been  previously  defined. 

Example  Part  3  :  The  degree  of  nonholonomy  of  the  2- trailer  convoy  is  4  at  points  whose 
coordinates  (x,  y,  0,  (pi,  (p2)  verify  v>i  f  mod  tt.  It  is  5  elsewhere. 

Summing  up  the  results  of  this  section  : 

The  method  we  use  for  testing  the  controllability  of  a  nonholonomic  system  at  a 
point  is  at  least  exponential  in  the  degree  of  nonholonomy  at  this  point. 


4.3  Growth  Vector 

This  section  introduces  the  growth  vector  of  a  controllable  nonholonomic  system  at  a  point.  It 
appears  in  [58].  This  concept  is  a  key  one  for  the  topological  viewpoint  that  we  wiU  adopt  in 
Section  6. 

Suppose  that  the  distribution  associated  to  our  system  is  (n  —  r)-dimensional.  Consider  a  point 
c  and  its  degree  of  nonholonomy  qc.  The  growth  vector  at  c  is  defined  as  the  sequence  (ni,  ...  ,  n,^), 
where  ni  =  (n  —  r)  <  n2  <  •  •  •  <  =  n  and  n,-  is  the  dimension  at  c  of  the  linear  space  generated 

by  combinations  of  brackets  of  degree  less  than  i. 

Example  Part  4  :  Let  us  recall  that  {Xi,X2,X3,X4,X6}  constitutes  a  basis  for  points  whose 
coordinates  (x,y,0,(pi,(p2)  verify  ¥>i  f  mod  ir,  while  {A’i,A’2,Ar3,X4,X9}  works  elsewhere.  One 
can  verify  (by  computing  the  dimension  of  the  linear  spaces  for  each  level)  that  the  growth  vector 
at  points  verifying  y>i  ^  f  mod  ir  is  (2, 3, 4, 5),  while  it  is  (2, 3, 4, 4, 5)  elsewhere. 
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4.4  Singularities  and  Regular  Systems 

All  the  above  tools  work  only  locally.  For  instance,  we  have  just  seen  that  the  growth  vector  is  not 
the  same  everywhere  in  the  manifold.  The  global  viewpoint  is  not  easy  to  reach.  A  first  step  is  to 
study  what  happens  in  a  neighborhood  of  a  point. 

A  filtration  {A,}  is  regular  at  a  point  c  if  the  growth  vector  is  constant  in  a  neighborhood  of 
c  [58]  [57].  TWs  means  that  all  the  ranks  of  A,(.)  are  constant  in  the  neighborhood.  Otherwise, 
the  filtration  is  singular  and  the  corresponding  point  is  a  singularity.  Most  of  the  problems  we 
encounter  when  we  try  to  define  growth  vectors  and  degrees  of  nonholonomy  derive  from  the 
presence  of  singularities.  By  extension,  we  will  say  that  a  system  is  regular  if  the  corresponding 
filtration  is  regular  everywhere. 

Example  Part  5  :  The  3  body  system  is  not  regular;  more  precisely  the  corresponding  filtration 
is  regular  at  points  verifying  |  mod  w.  It  is  singular  at  the  remaining  points.  Remark  that 
the  growth  vector  is  strictly  increasing  for  regular  points. 

For  regular  systems,  the  degree  of  nonholonomy  is  a  constant.  It  can  also  be  shown  (see  [51]) 
that  the  growth  vector  is  strictly  increasing,  so  the  procedure  we  designed  always  stops  in  that 
particular  case. 


4.5  Well-Controllability 


At  this  stage  of  the  presentation,  let  us  return  to  the  planning  problem.  This  section  introduces  the 
concept  of  well-controllability.  As  the  regularity  concept,  it  deals  with  the  existence  of  singularities, 
but  this  is  a  more  global  one. 

As  we  have  seen  in  Section  2,  a  general  idea  for  devising  a  nonholonomic  motion  planner  for 
controllable  systems  is  to  define  a  procedure  that  searchs  for  an  admissible  collision-free  path, 
taking  any  collision-free  path  as  a  seed  for  the  search^®. 

Very  recently  Lafferiere  and  Sussmann  proved  that  this  principle  is  a  general  one.  A  collision- 
free  path  is  first  computed  without  taking  the  nonholonomic  constraints  into  account.  Lafferiere 
and  Sussman’s  method  [55]  (see  Section  6.3  for  more  details)  roughly  consists  of  expressing  the 
first  holonomic  path  into  some  “local  coordinate  system”  (a  precise  definition  will  be  given  in 
Section  6.1);  from  these  coordinates,  because  the  system  is  controllable,  the  authors  show  that  it 
is  possible  to  explicitely  define  an  admissible  control  (and  then  an  admissible  path)  that  locally 
st^rs  the  system  from  a  given  point  (on  the  first  path)  to  any  other  on  the  first  path  inside  a  given 
neighborhood.  Because  the  planner  has  to  work  a  priori  everywhere,  one  has  to  define  a  procedure 
that  guarantees  to  find  a  local  coordinate  system  everywhere.  The  existence  of  such  a  coordinate 
system  is  a  technical  point  essential  for  the  method.  It  is  solved  by  considering  an  extended  system 
associated  with  the  original  one;  this  new  system  is  obtained  by  adding  virtual  controls  working  on 
vector  fields  defined  from  a  PH-family  of  the  original  system.  Since  the  nonholonomic  distribution  A 
is  (n-  ^)-dimensional,  it  seems  a  priori  that  k  additional  controls  would  suffice  to  make  the  system 
holonomic.  In  fact,  in  order  to  avoid  singularities  (understood  as  points  where  the  transformation 

“[28]  pinpointed  this  method  for  the  car-like  robot,  while  [32]  presents  a  planner  using  this  principle  for  this  case. 
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matrix  would  be  non  invertible),  one  bae  generally  to  add  more  controls,  Laiferriere  and  Sussmann 
note  also  that  additional  controls  make  easier  the  choice  of  a  transformation  matrix  with  a  good 
condition  number. 

Let  us  illustrate  this  point  using  our  example. 

Example  Part  6  :  Recalling  Example  Part  2,  a  local  coordinate  system  defined  from  {Xi,X2, 
X3,  X4,  Xe}  will  encounter  singularities.  Following  Laiferiere  and  Sussmann ’s  method,  a  possible 
extended  control  system  is  defined  by  {Jri,.y2»-^3»-X^4,  JTe, .STg};  in  the  process,  four  controls  are 
added  to  the  original  ones.  The  previous  results  show  that  it  is  everywhere  possible  to  choose  in 
this  family  a  basis  that  spans  R^. 

Now,  consider  the  following  family  : 

Uo  =  Xi 

Yi  =  [■X^ij-^2]  Ui  =  cos  (piXi  +  sin  ip\Yi 

Y2  =  [i/i,  Vi]  U2  =  cos  <{>2Ui  +  sin  (P2Y2 

Yz  =  [U2,V2] 

It  is  easy  to  check  (see  Appendix  3  for  the  general  case)  that  the  determinant  of  {Vb,  Vi,  V2,  U2,Yz} 
is  equal  to  1.  Therefore  {Vb,  V^i,  V^2j  V3}  spans  R®  every where^^ .  According  to  the  previous 
comments,  we  can  define  a  minimal  extended  system  that  never  meets  with  any  singularity.  More¬ 
over,  the  transformation  matrix  has  a  good  condition  number.  We  introduce  the  concept  of  a 
well-controllable  system. 

Definition  1  :  An  n-dimensional  nonholonomlc  system  defined  by  a  distribution  A  is  well- 
controllable,  if  there  exists  a  basis  of  n  vectors  fields  in  the  Control  Lie  Algebra  LA(A)  such  that 
the  determinant  of  the  basis  is  constant. 

Obviously  well-controllability  implies  controllability.  The  converse  does  not  hold.  Indeed,  as  we 
mentionned  in  Section  4.1,  a  system  can  be  controllable  while  the  local  degrees  of  nonholonomy  are 
unbounded.  This  means  that  the  filtration  {Af}  stabilizes  locally,  but  not  globally.  In  this  case,  it 
is  impossible  to  define  a  basis  verifying  the  conditions  of  our  definition. 

The  well- controllability  concept  is  a  global  one  and  it  is  related  to  the  planning  problem.  In¬ 
deed,  for  well-controllable  systems,  the  same  “local”  coordinate  system  can  work  everywhere.  This 
simplifies  Laiferiere  and  Sussmann ’s  planning  method.  But,  though  we  have  a  general  procedure  for 
testing  controUability,  we  have  no  general  procedure  for  testing  well-controllability.  For  instance, 
there  is  no  obvious  argument  leading  to  reducing  the  search  of  a  good  basis  to  a  small  family,  like 
a  PH-Hall  family. 

Definition  2  :  Let  B  be  a  basis  of  the  control  Lie  Algebra  verifying  the  conditions  of  Defini¬ 
tion  1.  The  degree  of  well-controllability  of  the  system  is  the  maximum  degree  of  all  the  elements 
ofB. 

Remark  :  If  the  system  is  well-controllable,  it  is  obvious  that  the  global  degree  of  nonholonomy 
is  finite. 

**Be  careful  :  the  degree  of  Y3  equals  8,  when  it  is  viewed  as  a  polynomial  function  with  indeterminates  Xi  and 
X2  in  CAi{Xi,X2]). 


Vo  =  X2 

Vi  =  siny)iAi  -  cos(f{Yi 
V2  =  sivnp2U\  —  C0S<P2Y2 
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Example  Paxt  7  :  The  2-trailer  convoy  is  a  weU-controDable  nonholonomic  system.  Its  degree 
of  nonholonomy  is  5,  while  its  degree  of  weU-controUability  is  at  most  8. 


4.6  Geometric  Models  and  Singularities  :  some  Examples 

Sin^arities  typically  come  from  the  geometry  of  the  system,  though  in  some  cases  a  bad  choice  of 
basis  might  create  artificial  singularities.  Let  us  illustrate  this  point  with  three  examples. 

Consider  once  again  the  case  of  the  1-trailer  convoy.  It  appears  (see  Appendix  2)  that  this  is  a 
regular,  weU-controUable  system  of  degree  3;  its  growth  vector  is  (2,3,4)  everywhere.  Now  consider 
the  same  convoy,  but  with  another  hooking  system.  Instead  of  hooking  the  traUer  exactly  at  the 
middle  of  the  rear  wheels,  we  hook  it  to  a  point  behind  the  rear  axle  (see  Figure  2)22.  Solving  the 
equations  yields  the  basis:  ° 


^  a' cos  9  ^ 

^  {a'  A-  a.  cos  yj)  cos  9  \ 

a'  sin  0 

and 

{o'  A-  a.  cos  (p)  sin  9 

U 

smif 

^  —  sin^p  j 

0  / 

If  we  consider  this  basis  only,  we  find  a  growth  vector  of  (2, 3, 4)  at  points  verifying  sin  ^  #  0. 
The  points  venfying  sin  ^  =  0  seem  to  constitute  a  singularity.  But,  checking  our  provisional  basis, 
we  find  that  the  vector  fields  are  coUinear  at  these  points,  though  the  distribution  is  regular  at 
these  points  too.  It  is  just  awkward  to  find  two  vector  fields  which  stay  independent  everywhere. 
For  the  record,  any  vector  field  of  the  form 


X  =  k 


^  a'  cos  9  ^ 

/ 

a' sm  9 

+  / 

0 

^  —  sinyj  y 

\ 

(a' -I-  a.  cos  y>)  sin^ 
sin^ 

0 


/ 


-|-  m 


0 

0 


^  a  cos  -f-  a'  j 


is  a  valid  vector  field.  With  some  ingenuity,  for  any  value  of  a  and  o',  it  is  possible  to  obtain  a 
global  basis. 


Now  let  us  pinpoint  a  stranger  case.  We  have  seen  that  the  3-body  convoy  we  study  is  con¬ 
trollable  with  degree  5  .  Appendix  2  shows  that  the  2-body  convoy  is  controllable  with  degree  3. 
The  1-body  convoy  (i.e.,  the  unicycle)  is  controUable  with  degree  2.  Because  of  the  singularity  we 
mentioned,  there  is  no  Ath  degree.  What  is  the  fundamental  difference  between  one  trailer  and  two 
trailers  ?  Furthermore,  we  prove  in  Section  5  that  a  n-body  convoy  is  well-controllable  and  that 
its  nonholonomy  degree  is  at  most  2".  What  is  its  precise  nonholonomy  degree  ?  What  kind  of 
singularities  do  we  stumble  upon  ? 

Finally,  our  third  example  doesn’t  model  any  robotics  systems  whatsoever2^  .  Rather,  it  tries 
to  capture  the  flavor  of  problems  encountered  in  practice  without  any  tedious  computations:  for 

“[31]  gives  a  constructive  proof  of  controllability  for  this  system. 

“This  example  was  suggested  by  Marc  Espie. 
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Figure  2:  Another  1-traJler  car  system 


instance,  the  example  of  the  2-trailer  system  is  quite  similar  to  this  one.  Let  us  work  in  Euclidean 
space  (a:,  y,  z).  Choose  an  integer  n  and  consider  the  following  vector  fields  : 


(  0 


Yo  = 


1  +  ^"  \ 

0  ) 


and  the  distribution  they  engender.  Computing  the  associated  filtration  obviously  sums  up  to 
computing 

/  (»«)(*)  \ 

n  =  I  I . 


Now,  this  system  is  regular  almost  everywhere  but  not  everywhere  :  the  point  (0, 0, 0)  is  a  major 
inconvenience.  At  that  point,  for  any  k  less  than  n,  the  vector  field  Yk  vanishes  altogether,  so  that 
the  growth  vector  at  (0,0,0)  is  (2,2,  ...  ,3),  giving  thus  a  generic  case  where  the  nonholonomy 
degree  is  arbitrarily  high.  Furthermore,  using  standard  techniques  (Partitions  of  Unity),  it  is  easy 
to  piece  together  a  denumerable  infinity  of  such  singular  patches,  so  that  the  resulting  ^stribution 
has  an  unbounded  degree. 

This  is  the  typical  case  where  our  Algorithm  will  not  terminate.  A  finer  study  of  the  problem 
would  be  a  tremendous  help. 


4.7  Nilpotent  and  Nilpotentizable  Systems 

We  have  seen  that  the  controllability  testing  procedure  does  not  necessarily  ternainate  in  the  general 
case.  Notwithstanding,  consider  the  following  special  case. 


Suppose  that  all  the  Lie  brackets  of  degree  greater  than  k  vanish.  In  this  case,  the  sequence  Xi 
stabilizes  :  ‘ 

XiCX2C‘-‘CXk=CA{X), 

We  can  stop  the  procedure  Controllability  as  soon  as  all  the  Lie  brackets  of  degree  k  or  less  are 
generated.  If  the  procedure  does  not  yield  a  basis,  then  the  system  is  not  controllable. 

Such  systems  are  called  nilpotent  of  order  k  (see  [6]  for  a  general  definition  of  the  concept  in 
the  Lie  Algebra  framework). 


Example  Part  8  :  In  our  example,  we  may  verify  (see  Appendix  2)  that  fATo,  fAro.Xill  =  -Xi 
Set  adx(y)  =  [X^Y],  Then^^:  adx2^”*(Xi)  =  (-1)”*  Afi.  The  system  is  not  nilpotent. 

In  some  cases,  a  non-nilpotent  system  can  be  transformed  into  a  nilpotent  one  via  a  linear 
change  of  controls  called  a  feedback  transformation.  Quite  logically,  such  systems  are  called  feedback 
nilpotentizable.  [55]  gives  some  examples  of  feedback  nilpotentizations  (e.g.,  the  unicycle,  a  car-like 
system  and  a  car-like  system  with  a  trailer).  See  also  [18]  for  sufficient  conditions  for  a  system  to 
be  nilpotentizable. 


Conjecture  :  Nilpotent  and  nilpotentizable  systems  are  well-controllable. 

The  study  of  this  conjecture,  currently  in  progress,  should  help  the  quest  for  a  general  test  of 
weU-controUability. 


4.8  Triangular  Systems 


The  concept  of  a  triangular  system  is  used  by  Murray  and  Sastry  in  [39].  A  system  is  triangular  \{ 
it  can  be  defined  as  : 


X\  =  V 

=  /2(a:i)» 

=  fz{x\,X2)v 

with  Xi  e  R”*'  and  J^m,-  =  n. 

t 

Because  of  the  triangular  form,  there  exists  simple  sinusoidal  control  that  may  be  used  for 
generating  motions  affecting  the  t***  set  of  coordinates  while  leaving  the  previous  sets  of  coordinates 

unchanged.  It  is  possible  to  use  these  sinusoidal  controls  for  planning  trajectories  (see  [39]  for 
details). 

Even  if  a  system  is  not  triangular,  it  may  be  possible  to  transform  it  into  a  triangular  one  by 
feedback  transformations.  Recent  work  [37]  shows  that  a  regular  nilpotent  controllable  system  can 
be  triangularized. 

This  example  appears  in  [55]  for  the  unicycle  and  the  car-like  robot,  i.e.,  systems  equivalent  to  our  current  system 
without  trailers  (see  Section  4). 
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Conjecture  :  Triangularizable  systems  are  well-controllable. 

The  next  section  will  mention  as  an  aparte  that  the  multibody  car  system  we  consider  along 
this  paper  is  triangulairizable. 
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5  The  Multibody  Car  System  is  Well- Controllable 


Let  us  return  now  to  the  general  case  of  our  convoy  with  n  trailers.  We  know  a  basis  {XajXt}  of 
the  distribution  A  of  the  system  (see  Section  3.2)  and  we  want  to  prove  the  existence  of  a  family  of 
fields  in  XA(A)  that  spans  the  full  tangent  space  when  applied  anywhere.  To  solve  this  problem, 
the  only  difficulty  is  to  find  a  good  family.  Moreover,  a  minimal  family  that  works  everywhere  (cf 
the  concept  of  well-controllability)  is  highly  desirable. 

Since  the  number  of  trailers  is  not  fixed,  we  have  to  compute  the  general  form  of  the  (n  -1-  3) 
coordinates  of  the  vector  fields  we  use. 

Recall  the  form  of  and  : 


Xa 


cosff 

sinff 

0 

—  sin  <pi 

sin  (pi  —  cos  <pi  sin  (p2 

/  •  *“2 
(siny>,_i  -  cosvj,_isinv’,)  J]  cos  <pj 

i=i 

.  ,  .  n-2 

(sin  (fin-l  —  cos  (Pn-1  SID  V’n)  FI  COS  (fij 

i=i 


The  Vector  fields  of  the  basis  are  obtained  through  the  use  of  the  same  combinations  as  in  Section 
4.6  (Example  Part  5). 


We  build  iteratively  three  brackets  of  degree  2‘  from  brackets  of  degree  2‘-i  : 


Step 


0 

Xi  =  [Yo,Zo] 

Yo  =  Xa 

II 

1 

Yi  =  cos  (piYo  -I-  sin  (piXi 

Zi  =  sin  (p{Yq  —  cos  <piX\ 

2 

X2  =  \Yi,Z\] 

Y2  =  cos  (P2Y1  -t-  sin  (P2X2 

Z2  =  sin  v’2Li  —  cos  <^2X2 

i 

Xi  = 

Yi  =  cos  (piYi^i  -f  sin 

Zi  =  sin  <PiYi^i  —  cos  <piXi 

The  general  form  of  the  vector  fields  coordinates  is  given  by  a  lemma  (Lemma  1,  in  Appendix  3). 
It  remains  for  us  to  choose  n  -1-  3  vector  fields  that  constitute  a  basis  of 

Lemma  2  :  For  any  point  c,  {Zq,  Zi,  Z2,  . . .  ,  Z„,Y„,  Xn+i)  (c)  spans  R"+3. 


Proof ;  We  just  have  to  exhibit  the  following  determinant  : 


0  13  P  P 

0  ^  13 

10  0  0 

1-10  0 
0  1-10 

0  0  1-1 


/?  cosQ  —sin a 

sin  a  cos  a 
0  0  0 

0  0  0 

0  0  0 

0  0  0 


00001  -1  0  0 


where  a  =  (^  -  *Pi)  do  not  need  to  express  fi).  It  appears  clearly  that  this  determinant 
equals  (-1)".  Q.KD. 


Finally,  since  the  determinant  does  not  vanish  anywhere,  we  can  conclude  : 


Property  :  The  multibody  car  systems  defined  in  Section  3.3  are  well-controllable,  with  degree 
at  most 

. . .  and  the  driver  of  our  luggage  carrier  can  play  rambling  through  the  airport  in  search  of  a  parking 
place  !  An  estimate  of  the  time  he  needs  to  park  his  vehicle  is  the  subject  of  the  following  section. 

Remark:  We  have  seen  that  such  a  system  is  not  nilpotent.  Because  of  the  presence  of 

singularities,  it  is  not  regular  either.  Nonetheless,  the  form  we  obtained  for  the  determinant  clearly 
shows  that  such  a  system  is  triangularizable. 
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6  The  Complete  Problem 


In  Section  2  we  saw  that  the  existence  of  an  admissible  collision-free  trajectory  for  a  controllable 
nonholonomic  system  is  characterized  by  the  existence  of  a  collision-free  trajectory  for  the  associated 
holonomic  system.  Planning  a  trajectory  can  thus  be  achieved  through  the  following  steps: 

•  using  a  geometric  planner  for  finding  a  trajectory  without  taking  into  account  nonholonomic 
constraints^®  ,  and  then 

•  steering  the  system  as  close  by  as  possible  to  this  trajectory  along  an  admissible  trajectory. 

Such  a  general  strategy  has  been  refined  into  two  different  approaches  that  we  will  examine 
in  Sections  6.3  [55]  and  6.4  [33].  The  first  one  uses  a  constructive  proof  of  local  controllability 
founded  on  fundamental  tools  of  differential  geometry  introduced  in  the  next  section  (Section  6.1). 
The  second  one  uses  an  explicit  form  for  canonical  feasible  trajectories  (e.g.,  shortest  trajectories)! 

Both  approaches  come  up  against  the  following  key  problem  :  how  to  guarantee  that  the 
admissible  trajectory  lies  as  close  to  the  steering  trajectory  as  the  obstacles  require  ?  The  topological 
nature  of  this  question  is  discussed  in  Section  6.2. 


6.1  FVom  Vector  Fields  to  Trajectories 

In  this  subsection  we  will  investigate  the  delicate  problem  of  finding  trajectories  that  are  compatible 
with  our  nonholonomic  constraints.  We  need  some  precise  concepts  of  differential  geometry,  which 
are  thoroughly  studied  in  [50]  [57]  [51]  [52]. 

Let  us  first  take  the  very  simple  example  of  an  Euclidean  plane.  At  any  point,  the  tangent  space 
is  a  two-dimensional  Euclidean  vector  space,  though  one  should  not  confuse  the  tangent  space  with 
the  manifold  itself.  Pictorially,  just  view  the  Euclidean  plane  as  a  table  and  the  tangent  space  at 
a  point  as  a  sheet  of  paper  glued  to  the  table  at  this  very  precise  point.  Considering  the  tangent 
space  at  another  point  just  involves  sliding  the  sheet  and  gluing  it  at  the  other  point.  One  can  then 
take  a  distinguished  system  of  coordinates  in  these  tangent  spaces,  namely  the  constant  vectors  X 
and  Y.  These  vectors  induce  a  system  of  coordinates  on  the  manifold  in  a  straightforward  way. 
Starting  from  a  given  point  (the  origin),  just  move  in  the  X  direction  for  a  given  time,  say  a,  then 
in  the  Y  direction  for  a  time  6.  You  have  reached  the  point  of  cartesian  coordinates  a  and  b.  There 
are  some  alternate  ways  to  achieve  that  :  you  could  have  first  followed  Y  for  a  time  6,  then  X 
for  a  time  o  or,  with  self-assurance,  have  directly  taken  the  direction  of  choice,  the  str^ght  line 
along  aX-^bY.  We  c^  deduce  some  interesting  facts  from  these  simple  manipulations.  First,  each 
of  these  procedures  gives  rise  to  a  system  of  coordinates.  That  means  that  we  have  a  one-to-one 
correspondence:  every  point  is  defined  by  its  coordinates  and  reciprocally.  That  also  means  that 
the  correspondence  is  smooth,  two  neighboring  points  have  neighboring  coordinates  and  the  whole 
scheme  is  thoroughly  unsurprising. 

“Finding  such  a  trajectory  corresponds  to  the  classical  Piano  Mover  problem.  This  problem  is  decidable.  From  a 
thTOretical  pomt  of  view,  general  algorithms  exist  in  the  Kterature.  Notwithstanding,  at  this  time,  there  is  no  general 
software  that  tuns  efficiently  in  practice.  This  is  due  to  the  intrinsic  combinatorial  complexity  of  the  problem. 
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We  are  now  going  to  use  the  same  scheme  for  our  purposes.  The  main  difference  is  that  our 
vector  fields  won’t  be  constant  any  longer,  and  won’t  even  need  to  be  globally  defined.  We  could 
say  that,  instead  of  dealing  with  a  flat  Euclidean  space,  we  will  now  work  in  a  skewed  space,  where 
vector  fields  are  allowed  to  behave  horribly.  But,  nonetheless,  the  same  constructions  wEl  work 
locally  and  give  rise  to  some  systems  of  coordinates — these  are  no  longer  equivalent. 

Choose  a  point  p  in  our  manifold  and  a  vector  field  X  defined  around  this  point.  There  is 
exactly  one  trajectory  7(t)  starting  at  this  point  and  following  In  a  formal  notation,  it  verifies 
7(0)  =  p  and  7(t)  = 

One  defines  the  exponential  of  X  (denoted  by  e^)  to  be  the  point  7(1).  This  gives  a  corre¬ 
spondence  between  the  space  of  vector  fields  and  a  neighborhood  of  p.  It  is  obvious  that  one  has 
gtx  _  namely  this  definition  doesn’t  describe  a  peculiar  point  of  a  trajectory  but,  more  ac¬ 
curately,  links  every  point  of  the  trajectory  to  a  specific  vector  field.  Let  us  translate  our  previous 
example  to  this  formalism.  Following  aX  -|-  bY  for  a  given  time  (the  unit  time)  simply  means  to 
take  Following  X  for  a  time  a  amounts  to  following  aX  for  the  unit  time,  that  is  taking 

c“^,  and  following  Y  for  a  time  b  is  the  same  as  taking  This  is  still  a  slightly  different  point  of 
view  :  instead  of  considering  the  exponential  to  define  a  specific  point  of  a  trajectory  with  regard 
to  an  origin  point  p,  we  understand  it  as  describing  a  motion  from  a  point  to  another  on  a  given 
trajectory.  Thus,  starting  at  the  origin  o,  following  aX  for  a  given  time,  then  bY  leaves  us  at  the 
point  •  c“^  •  o  Therefore  the  exponential  of  a  vector  field  X  appears  as  an  operation  on  the 
manifold,  meaning  “slide  from  the  given  point  along  the  vector  field  X  for  unit  time.” 

In  that  setting,  everything  works  nearly  as  smoothly  as  in  the  Euclidean  case,  at  least  locally. 
The  main  difference  is  that,  whenever  [X,y]  ^  0,  following  directly  aX  +  bY  or  following  first 
aX  then  bY  are  no  longer  equivalent.  Intuitively,  [X,Y]  measures  the  variation  of  V  along  the 
trajectories  of  X;  in  other  words,  the  field  Y  we  follow  in  aX  -|-  bY  has  not  the  same  value  as  the 
field  Y  we  follow  after  having  followed  aX  (indeed  Y  is  not  evaluated  at  the  same  points  in  both 
cases).  The  main  result  is  the  following  : 

Assume  that  X\ , . . . ,  Xn  are  vector  fields  defined  in  a  neighborhood  U  of  &  point  p  such 
that  at  each  point  of  U,  Xi,...,Xn  constitutes  a  basis  of  the  tangent  space.  Then  there  is  a 
smaller  neighborhood  V  of  p  on  which  the  correspondences  (ai,...,a„)  1-^ 

(oi, . .  .,o„)  •-►  .p  are  two  coordinate  systems,  called  the  first  and  the  second  normal 

coordinate  system  associated  to  { , . . . ,  X„}. 

The  Campbell-Haussdorf-Baker-Dynkin  formula  states  precisely  the  difference  between  the  two 
systems  : 

For  a  sufficiently  small  t,  one  has  :  e*^  •  where  €(t)  0  when 

<^0. 

Actually,  the  whole  Campbell-Haussdorf-Baker-Dynkin  formula  as  proved  in  [57]  gives  an  ex¬ 
plicit  form  for  the  c  function.  More  precisely,  €  yields  a  formal  series  whose  coefficients  lie  in 
CA{{X,  y})  :  the  coefficient  of  is  a  combination  of  brackets  of  degree  k.  In  the  case  of  a  nilpotent 
system  of  order  fc,  since  brackets  of  degrees  greater  than  k  vanish,  the  Campbell-Haussdorf-Baker- 
Dynkin  gives  an  exact  development  of  the  exponential.  This  property  is  used  in  the  Lafferiere  and 
Sussmann’s  planner  presented  below. 
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Roughly  speaking,  the  Campbell-Haussdorf-Baker-Dynkin  formula  tells  us  how  a  nonholonomic 
system  can  reach  any  point  in  a  neighborhood  of  a  starting  point.  This  formula  is  the  hard  core 
of  the  local  controllability  concept.  It  yields  a  method  for  explicitly  computing  trajectories  in  a 
neighborhood  of  a  point. 

Now  we  take  a  closer  look  at  the  problem  of  obstacle  avoidance. 


6.2  Obstacle  Avoidance  ;  the  Topological  Question 
6.2.1  What  Kinds  of  Topologies  ?  An  Informal  Statement 

Let  us  come  back  to  the  sources.  Motion  planning  in  Robotics  deals  with  obstacle  avoidance.  Real 
obstacles  get  transformed  into  “obstacles”  in  the  configuration  space.  The  HausdorfF  metric^®  on 
the  bodies,  which  are  closed  compact  subsets,  in  the  environment  (that  is  to  say,  3-dimensional 
Euclidean  space)  induces  a  metric  in  the  configuration  space.  Therefore,  the  reference  open  sets 
an  the  configuration  space  are  the  open  sets  in  the  topology  induced  by  the  HausdoriF  metric  in 
Euclidean  space.  This  is  the  topology  needed  for  solving  placement  problems^^.  A  path  appears 

as  a  continuous  function  from  a  closed  interval  of  R  to  the  configuration  space  equipped  with  this 
topology. 

In  some  c^es,  the  very  act  of  considering  motions  can  lead  to  a  finer  topology.  If  we  introduce 
the  distance  induced  by  the  best  trajectory(ies)  between  two  points  with  respect  to  a  given  cost 
(length,  energy,  time  taken,  etc),  differential  considerations  take  the  scene.  Consider  the  case  of 
energy  for  instant.  For  holonomic  systems,  since  every  smooth  path  in  the  configuration  space  is 
an  admissible  trajectory,  this  cost  induces  a  natural  Riemannian  distance.  In  that  case,  the  induced 
topology  remains  the  same. 

For  general  nonholonomic  systems,  there  may  exists  points  at  an  infinite  distance  of  each  other. 
The  non-holonomic  constraints  partition  the  configuration  space  into  disconnected  submanifolds, 
and  the  resulting  topology  has  little  ressemblance  to  the  natural  one.  However,  for  controUable 
nonholonomic  systems,  any  two  points  can  be  joined  by  an  admissible  trajectory;  considering  the 
best  trajectory  leads  once  again  to  a  weU-defined  metric.  This  is  the  origin  of  sub-Riemannian 
geometry.  Recent  contributions  show  that  Riemannian  and  sub-Riemannian  metrics  are  equivalent. 
Therefore  both  topologies  are  the  same.  The  next  section  state  these  resoilts  more  thoroughly. 


6.2.2  Of  Sub-Riemannian  Metrics,  Shortest  Paths  and  Geodesics 

This  section  makes  use  of  the  ideas  given  in  [58]  for  the  case  of  regular  systems.  Consider  a 
controUable  nonholonomic  system  (i.e.,  a  completely  nonholonomic  one)  defined  on  a  n-dimensional 

“A  Hau^orff  metric  can  be  defined  for  any  space  equipped  with  a  distance  d.  It  yields  the  foUowing  topology  on 
compact  subsets  ;  o  r 

dH{A,B)  =  infsexisupyeYd(x,y)). 

”See  the  problem  of  doth  or  leather  cutting  as  an  example  of  such  a  problem  in  the  context  of  Computational 
Geometry. 
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manifold  CM  (for  “Configuration  Manifold”)  by  a  distribution  A.  The  nonholonomic  metric^®[58] 
is  defined  by 

^a(c,cO=  dt, 

-1^S{c,e>)  Jo 

where 

5(c,c0  =  {7  I  [0, 1]  CM,  7(0)  =  c,  7(1)  =  c',  7  €  A} . 

In  that  setting,  geodesics  are  admissible  trajectories  that  locally  solve  the  variational  problem. 

The  proof  of  the  equivalence  of  the  Riemannian  and  the  sub-Riemannian  metric  resides  in  a 
two-sided  estimate  of  the  size  of  sub-Riemannian  bails.  Denote  by  B({c)  the  sub-Riemannian  e-ball, 
i.e.,  the  set  of  points  reachable  from  c  by  an  admissible  trajectory  of  length  less  than  e.  Assume 
that  the  system  is  regular  at  c  (see  Section  4.8).  Let  {c,}  be  the  local  coordinate  system  defined 
in  Section  6.1.  Now  consider  the  parallelepiped  Pa,t(c)  =  €  CM  \  |c,(c')|  <  |  where  tp{i) 

is  derived  from  the  growth  vector  {ni,. .  .,n,j}  of  A  at  c  as  (p(i)  =  j  for  n_,_i  <i  <  rij^.  A  two 
sided-estimate  of  Be{c)  is  given  by  the  parallelepiped  theorem  [58]^  :  there  are  positive  constants 
oc2i  C07  such  that  for  e  <  cq, 


BQj^e(c)  C  Re(c)  C  -P oi2,c(^)- 


Therefore,  for  regular  systems,  Riemannian  and  sub-Riemannian  topologies  are  the  same. 

Remark  :  The  parallelepiped  theorem  holds  only  at  regular  points.  There  are  some  technical 
problems  at  singular  points  (e.g.,  as  at  the  singularities  appearing  in  the  2-body  system).  It  seems 
possible  to  extend  the  proofs  (using  a  local  coordinate  system,  i.e.,  valid  everywhere  in  a  neighbor¬ 
hood  of  a  regular  point)  to  well-controllable  systems  by  using  a  local  coordinate  system  holding  for 
singular  points  as  well  as  for  regular  points  (such  a  system  exists  by  definition).  Nevertheless,  as 
always  with  singularities,  some  care  will  have  to  be  taken. 

Strange  phenomena  appear  in  this  sub-Riemannian  geometry  framework.  We  know  that,  in 
general,  if  shortest  paths  are  geodesics,  geodesics  are  not  necessarily  shortest  paths — ^indeed,  they 
only  minimize  length  locally.  One  of  the  main  features  of  Riemannian  geometry  is  that,  locally, 
geodesics  are  shortest  paths.  Consider  the  c-ball  ■0c(c)  above,  and  •S'e(c)  its  boundary  sphere.  In 
Riemannian  geometry,  for  e  sufficiently  small,  the  sphere  5^  is  in  one  to  one  correspondence  with 
the  ends  of  geodesics  of  length  c  (the  so-called  wave  front).  No  similar  property  does  hold  for 
nonholonomic  systems.  [58]  holds  two  drawings  illustrating  the  strange  relationship  between  the 
spheres  and  the  wave  fronts  in  the  Heisenberg  group  case. 

’*Thi8  metric  is  also  known  as  a  singular^),  a  Camot-Caratheodory]i^,  or  a  sud-Aiemannion [51]  metric. 

^Example  Part  9  :  For  the  2-trailer  convoy  at  c  =  (0, 0, 0, 0, 0),  the  growth  vector  is  (2, 3, 4, 5)  (see  Example  Part 
4).  Therefore  : 

Po.«(c)={(a!,»,«,»>i,¥>2)||*|<o«,  |»|<o<,  \e\<a£^,  |v>i|<ac®, 


Bibliographic  note  :  the  proof  of  this  theorem  does  not  appear  in  [58].  A  proof  using  different  terminology 
appears  in  [51]. 
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Another  strange  phenomenon  can  be  illustrated  by  the  study  of  the  car-like  system^i.  In  this 
particiUar  <^e,  the  shortest  paths  have  an  explicit  form  (see  the  foUowing  section).  Figure  3  is  a 
rendering  of  one  of  the  corresponding  spheres.  Just  notice  that  this  sphere  is  not  smooth  and  look 
at  the  planes  cutting  the  smooth  part. 


6.2.3  Geodesics  and  Shortest  Paths  :  Elementary  Computational  Aspects 

The  dassical  way  for  computing  geodesics  is  to  use  the  maximum  principle  of  Pontryagin  [431 
This  IS  a  powerful  t^l  from  classical  Optimal  Control  Theory,  which  provides  necessary  conditions 
or  the  existence  of  an  optimal  control.  The  bang-bang  principle  is  its  direct  consequence  fll. 
Under  some  hypoth^es,  this  principle  gives  the  form  of  the  optimal  controls,  if  they  exist.  It  has 
been  apphed  m  [19]  for  Hilare-Uke  robots  (see  Section  3.3.2)  :  in  this  case,  geodesics  are  piecewise 
clothoids  or  Mticlothoids.  Using  the  same  ideas,  we  may  verify  that  the  geodesics  for  car-like  robots 
(see  5)ection  3.3.3)  are  piecewise  arcs  of  circle  or  straight  lines. 

How  to  compute  shortest  paths  is  a  much  more  difficult  problem,  even  when  an  explicit  form 
for  the  gwdesics  (local  shortest  paths)  is  known.  The  problem  is  basically  a  combinatorial  one  : 
ow  to  piece  smooth  geodesic  parts  together  in  order  to  produce  a  shortest  path  ?  As  far  as  the 

been  answered  for  the  car-like  robot  by  Reeds  and  Shepp 
[  ].  They  ^tend  the  work  of  Dubins  on  the  form  of  smooth  shortest  paths  [14]  and  they  establish 
precisely  which  combinations  of  arcs  of  circle  and  straight  line  segments  can  produce  shortest  paths. 

Since  the  number  of  used  combinations  is  finite,  this  gives  birth  to  an  efficient  method  to  compute 
shortest  paths,  ^ 


6.3  A  Planner  Using  Philipp  Hall  Coordinate  Systems 


Tfos  section  is  only  a  sketch  of  the  general  approach  developed  by  Lafferiere  and  Sussmann  in 
[65],  First,  they  study  nilpotent  and  nilpotentizable  systems.  For  such  systems  we  have  seen 
that  it  IS  possible  to  compute  a  basis  B  of  the  Control  Lie  Algebra  ZA(A)  from  a  Philipp  Hall 
lanmy.  Their  method  assumes  that  a  holonomic  trajectory  7  is  given.  H  we  express  locally  this 

rajectory  on  B,  i.e.,  if  we  write  the  tangent  vector  7(t)  as  a  linear  combination  of  vectors  in  Bh(t)) 
the  resulting  coefficients  define  a  control  that  steers  the  holonomic  system  along  7.  Using  the 
C^pbeU-Haussdorf-Baker-Dynkin  formula,  it  is  then  possible  to  compute  an  admissible  control 
u  for  the  nonholonomic  system  that  steers  the  system  exactly  to  the  goal  (indeed,  since  all  the 
brackets  vanish  ^ter  a  given  level  k,  the  CampbeU-Haussdorf-Baker-Dynkin  formula  gives  an  exact 

evelopment  of  the  exponential  on  brackets  of  degree  less  than  fc,  so  the  synthetized  trajectory  ends 
exactly  at  the  same  point. 


For  a  general  system,  Lafferiere  and  Sussmann  reason  as  if  the  system  were  nilpotent  of  order  k 
In  this  case,  the  s)mthetized  trajectory  deviates  from  the  goal.  Nevertheless,  thanks  to  a  topological 

property,  this  basic  method  is  used  in  an  iterated  algorithm  that  produces  a  trajectory  ending  as 
dose  to  the  goal  as  wanted.  ^ 


we  allow  further  inequality  constraints 
to  the  sub-Rlemannian  geometry  framework. 


on  the  controls  (see  Remark  in  Section  3.3.3),  [5]  shows  how  to  relate 
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In  both  cases,  the  nonholonomic  trajectory  is  a  local  approximation  of  7.  In  the  presence  of 
obstacles,  the  “gap”  due  to  the  approximation  has  to  be  estimated  in  order  to  avoid  the  obstacles. 
This  point  is  not  clearly  mentioned  in  the  preliminary  version  of  [55].  In  fact,  their  algorithm  can 
be  used  as  is  by  performing  a  recursive  subdivision  on  the  holonomic  trajectory  until  all  endpoints 
can  be  linked  by  collision-free  trajectories  synthetized  by  their  algorithm  above  (this  idea  is  also 
used  in  the  second  planner  described  below).  It  also  seems  that  the  parallelepiped  theorem  could 
be  used  to  prove  that  the  subdivision  procedure  always  stops. 

Remark  :  Laiferiere  and  Sussmann’s  strategy  can  be  extended  to  well-controUable  systems  by 
replacing  the  Philip  Hall  local  coordinate  system  by  another  one  built  from  any  basis  working 
everywhere.  This  extension  is  currently  under  study. 

6.4  A  Planner  Using  Shortest  Paths 

The  planner  developed  by  Jacobs  and  Laumond  [33]  uses  a  shortest  paths  approach.  The  three 
steps  of  the  algorithm  are  as  follows  : 

•  Plan  a  trajectory  7  for  the  corresponding  holonomic  system.  If  one  does  not  exist,  then  no 
feasible  trajectory  exists. 

•  Subdivide  7  until  all  endpoints  can  be  linked  by  a  minimal  length  collision-free  feasible  tra¬ 
jectory. 

•  Run  through  an  “optimization”  routine  to  reduce  the  length  of  the  trajectory. 

The  convergence  of  the  algorithm  is  a  consequence  of  the  parallelepiped  theorem.  Indeed,  consider 
a  point  c  on  7  and  Afc  a  neighborhood  of  c  included  in  the  collision-free  configuration  space.  The 
parallelepiped  theorem  guarantees  that  there  is  a  neighborhood  of  c,  such  that  for  each  point 
c'  €  Afc  the  shortest  trajectory  between  c  and  <f  lies  in  Afc-  Therefore  since  7  can  be  covered  by  a 
finite  number  of  such  neighborhoods  Afcy  the  subdivision  procedure  will  stop. 

This  planner  has  been  totally  implemented  in  an  exact  version  for  the  special  case  of  a  polyg¬ 
onal  car-like  robot.  This  means  we  had  to  implement  a  geometric  planner  for  computing  an  exact 
representation  of  the  collision-free  configuration  space.  To  do  this,  we  used  Avnaim  and  Boisson- 
nat’s  algorithm  [2].  We  also  implemented  a  procedure  for  computing  the  shortest  trajectories  in 
the  absence  of  obstacles  based  upon  Reeds  and  Shepp’s  work.  Finally,  we  designed  a  fast  collision 
checking  procedure.  Figure  4,  an  excerpt  from  [33],  shows  how  the  algorithm  responds  to  the  clas¬ 
sical  example  of  the  parking  problem.  The  three  drawings  give  the  trajectories  produced  by  the 
three  steps  of  the  algorithm. 

An  advantage  of  such  a  strategy  is  that  it  optimizes  locally  the  trajectory  (in  terms  of  the 
trajectory  length).  Indeed,  the  third  step  finds  a  quasi-optimal  solution.  Notice  that  finding  the 
optimal  one  is  a  problem  known  to  be  very  difficult,  and  computationally  very  complex. 

The  main  drawback  of  this  general  strategy  is  that  we  need  to  compute  the  shortest  paths  (see 
Section  6.3.3).  We  are  working  on  the  Hilare-like  system  :  at  this  time  we  know  an  explicit  form 
for  the  geodesics  [19],  but  not  for  the  shortest  paths. 
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Notwithst^ding,  using  general  mathematical  techniques  for  proofs  of  controllability  and  com¬ 
plexity  analysis,  we  have  laid  a  theoretical  basis  for  a  study  of  systems  of  greater  complexity. 
Essentially,  the  method  consists  in  establishing  a  catalogue  of  canonical  trajectories  having  the 
necessary  topological  properties,  then  in  using  them  together  with  the  subdivision  motion  planning 
technique.  Clearly,  the  main  question  is  :  how  to  compute  a  sufficient  set  of  canonical  trajectories  ? 
In  the  general  case,  the  only  way  out  seems  to  be  the  use  of  discretization  techniques,  but  even  in 
this  case,  the  theoretical  background  is  required  to  find  a  discretization  fine  enough  to  solve  the 
problem.  This  latter  aspect  is  currently  under  investigation. 


6.5  Complexity  of  the  Complete  Problem 


As  discussed  in  Section  2,  the  decision  part  of  the  motion  planning  problem  for  controllable  systems 
is  eqmvalent  to  the  decision  problem  for  the  associated  holonomic  system.  Hence  the  complexity 
of  this  problem  is  a  polynomial  function  of  the  complexity  of  the  environment  (i.e.,  the  number 
of  geometric  primitives  required  to  describe  it),  and  the  classical  algorithms  for  the  piano  movers 
problem  can  be  applied. 


Notwithstanding,  the  complexity  of  the  complete  problem  (i.e.,  actually  producing  a  trajectory) 
is  more  difficult  to  grasp.  As  a  rule,  any  measure  of  complexity  for  this  problem  ought  to  be  bounded 
from  below  by  the  complexity  of  the  solution,  which  in  turn  ought  to  account  for  the  number  of 
singular  points  (cusps,  loops,. . .)  it  contains.  Unfortunately,  this  number  bears  no  relationship  to 
the  customary  description  of  the  input  data  used  for  the  piano  movers  problem.  It  actually  depends 
on  the  inner  size  of  the  free  space,  measured  in  the  sub-Riemannian  setting  adapted  to  the  problem. 


Before  giving  a  formal  definition  of  the  complexity  of  a  trajectory,  as  it  appears  in  [5],  we  will 
first  introduce  the  concept  for  a  continuous  function  /:  [a,  6]  ^  R.  A  good  measure  of  what  we  call 
the  geometric  complexity  GC{f)  of  such  a  function  is  the  number  of  changes  of  variation.  More 
precisely,  this  complexity  is  defined  as  the  quotient  of  the  total  variation  of  the  function  (see  [45]) 
by  the  amplitude  of  the  function. 


In  the  current  context,  our  trajectories  depend  on  the  associated  controls,  which  are  continuous 
real  valued  functions.  Consequently,  we  define  the  complexity  of  a  trajectory  according  to  the 
complexity  of  the  associated  controls.  H  a  trajectory  7  is  defined  by  a  control  function  u,  its 
geometric  complexity  GC('y)  is  : 


GCi'f)  = 

denoting  the  geometric  complexity  of  the  continuous  real  valued  function  («,0»  projec¬ 
tion  of  u  along  the  axis  supporting  ^  (see  [5]  for  details). 

This  definition  gr^ps  every  critical  point  of  a  trajectory,  and  so  matches  closely  the  output 
complexity  of  any  motion  planner.  In  the  current  context,  it  also  includes  the  number  of  maneuvers 
as  well  as  the  number  of  loops  and  inflexion  points. 

This  definition  hints  at  some  lower  bounds  for  the  complexity  of  complete  motion  planners.  For 
instance  [5]  solves  the  case  of  the  car-like  robot.  Given  c  and  cf  two  configurations  in  the  same 
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connected  component  of  configuration  space,  we  proceed  to  define  c.  Choose  an  admissible  path 
between  c  and  .  At  a  point  q  of  that  path,  find  the  Euclidean  diameter  of  the  largest  ball  centered 
at  q  and  wholly  contained  in  the  free  configuration  space.  Take  the  infimum  of  this  diameter  for 
all  points  of  the  path.  Then  f  is  defined  as  the  supremum  of  this  quantity  over  aU  possible  paths 
linking  c  and  d. 

Then  the  complexity  of  a  trajectory  of  a  car-like  robot  going  between  c  and  d  is  proportional 
to  0(€-2). 

Roughly  speaking,  this  means  that,  in  the  car  parking  problem  (as  shown  in  Figure  4),  the 
number  of  necessary  maneuvers  asymptotically  varies  as  the  square  of  the  inverse  of  the  margin  of 
maneuver. 

The  proof  of  this  result  is  based  upon  the  Campbell-Haussdorf-Baker-Dynkin  development  of 
the  exponential.  A  detailed  study  of  the  general  case  exhibits  very  close  relationship  between  this 
complexity  model  and  the  degree  of  nonholonomy  of  the  system.  Everything  depend  upon  the  shape 
of  the  parallelepipeds  derived  from  the  growth  vectors  (Section  6.2.2).  A  general  proof,  currently 
under  study,  should  lead  to  the  following  property  : 

Property  :  Given  c,  d  two  configurations  in  the  same  arcwise-connected  component  of  the 
configuration  space.  Define  c  as  the  supremum,  over  all  continuous  paths  from  c  to  </,  of  the 
infimum,  over  all  configurations  q  along  that  path,  of  the  Euclidean  diameter  of  the  largest  ball 
centered  at  q  and  wholly  contained  in  the  free  configuration  space.  The  problem  of  finding  the 
trajectory  for  a  regular  controllable  nonholonomic  system  of  degree  k  between  c  and  d  is  in  0(c“*). 

Conjecture  :  The  problem  is  in  0(f“*)  for  well-controllable  systems  whose  well-controllability 
degree  equals  k. 

If  our  conjectures  hold,  this  would  mean  that  our  luggage  carrier  driver  would  have  to 
do  0(c“("+^))  maneuvers  in  order  to  park  his  machine  in  the  neighborhood  of  regular 
points,  and  in  in  the  worst  case  ... 
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7  Appendix  1  :  Related  Work  and  Background 


This  section  presents  an  overview  of  results  recently  obtained  in  constrained  motion  planning. 
They  are  complementary  to  those  mentioned  in  the  main  paper,  in  the  extent  that  they  take  a 
computational  point  of  view  into  account. 

7.1  Car-like  and  T^ailer-like  Robots 

[28]  envisaged  the  controllability  of  a  car-like  robot.  The  main  result  is  that  the  existence  of  a 
collision  free  trajectory  for  such  a  system  is  characterized  by  the  existence  of  a  connected  component 
in  the  free  configuration  space.  The  constructive  proof  of  this  result  has  led  to  the  implementation 
of  a  planner  [32].  This  planner  uses  an  approximate  representation  of  the  free  configuration  space 
decomposed  into  parallelepipeds. 

The  planner  described  in  [3]  uses  a  discretization  of  the  phase  space,  the  adjacency  relation 
between  two  cells  taking  kinematic  constraints  into  account.  Collision  tests  are  obtained  from  a 
bitmap  representation  of  the  2D  environment.  The  search  algorithm  applies  a  best-first  search 
strategy  whose  cost  function  is  the  number  of  maneuvers.  The  planner  then  produces  paths  with 
a  minimal  (for  the  discretized  representation)  number  of  maneuvers. 

Notice  the  heuristic  approach  developed  in  [56]  :  the  2D  environment  is  decomposed  into  a 
set  of  corridors  in  which  a  specific  technique  for  planning  smooth  paths  is  applied.  Finally  [59] 
proposes  an  algorithm  that  produces  a  motion  with  the  minimum  number  of  turns  for  a  set  of  lanes 
extracted  from  the  environment. 

The  case  of  trailer-like  robots  was  attacked  by  Laumond  and  Simeon  [31];  they  established 
the  controllability  of  such  systems  by  applying  the  Lie  Algebra  formalism  briefiy  recalled  in  the 
introduction.  They  also  give  a  second  constructive  proof,  but  this  one  does  not  lead  to  an  efficient 
algorithm. 

The  Barraquand  and  Latombe  planner  cited  above  also  applies  to  the  case  of  a  robot  with 
trailers.  Their  planner  is  thus  the  first  to  produce  paths  for  such  a  system. 

7.2  Smooth  Paths 

Planning  smooth,  maneuver-free,  paths  for  a  mobile  robot  appears  to  be  a  more  difficult  problem 
than  when  we  allow  maneuvers.  In  fact,  there  is  no  comparable  controllability  result.  It  is  indeed 
possible  that  a  path  exists  for  a  holonomic  system,  and  yet  no  smooth  path  exists.  The  specific 
problem  was  first  addressed  by  Dubins  [14],  who  gives  the  form  of  the  shortest  bounded  curvature 
path  in  the  absence  of  obstacles.  The  problem  of  obstacle  avoidance  appears  more  recently  in  [28]. 
In  [29],  the  environments  consist  of  closed  curves  which  are  not  necessarily  polygons.  Unfortunately, 
the  method  presented  there  is  not  guaranteed  to  find  a  path.  In  Fortune  and  Wilfong  [15],  a  decision 
algorithm  is  ^ven  to  decide  if  a  path  exists  under  given  conditions.  The  algorithm  is  exact,  but 
does  not  generate  the  path  in  question.  This  algorithm  runs  in  exponential  time  and  space.  The 
algorithm  in  [21]  is  a  provably-good  approximate  algorithm  solving  the  problem.  It  depends  on  a 
simplification  of  the  set  of  smooth  trajectories  to  a  sufficient  subset  of  canonical  trajectories. 
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7 .3  Shortest  Paths 


The  study  of  shortest  paths  for  constrained  systems  began  with  the  seminal  work  of  Dubins  [14], 
who  found  the  form  of  minimal  length  smooth  paths  with  bounded  average  curvature  for  the 
nonholonomic  system  we  study  in  this  work.  He  established  his  result  in  the  case  that  there  are 
no  obstacles.  The  algorithm  presented  in  [21]  is  a  provably  good  approximate  algorithm,  which 
depends  upon  an  extension  of  the  results  of  Dubins  to  the  case  where  there  are  obstacles  in  the 
environment.  This  extension  has  been  presented  in  a  rigorous  manner  in  [20].  When  there  are 
obstacles,  mimmaJ-length  smooth  paths  consist  of  paths  of  the  forms  given  by  Dubins,  alternating 
with  parts  of  the  obstacle  boundaries.  Finally,  the  same  proof  holds  for  a  polygonal  robot  moving  in 
a  polygonal  environment.  However,  in  this  case  the  portions  of  the  obstacle  boundaries  are  replaced 
by  logarithmic  curves  which  we  call  generalized  tractrices.  These  curves  are  those  that  maintain 
contact  between  an  obstacle  edge  and  a  robot  vertex  while  satisfying  nonholonomic  kinematic 
constraints  [56].  A  study  is  currently  in  progress  to  exploit  this  characterization  of  the  minimal- 
length  paths  and  develop  a  provably  good  algorithm  finding  smooth  paths  for  a  polygonal  robot. 

Recently  Reeds  and  Shepp  extended  the  work  of  Dubins  to  piecewise  smooth  paths  with  bounded 
curvature,  alas  without  obstacles  [44].  That  is,  they  allow  maneuvers  and  cusps  along  the  path. 

Essentially  they  proceed  to  show  that  any  path  holding  more  than  two  cusps  is  reducible  to  a 
path  with  at  most  two  cusps,  no  longer  and  possibly  shorter.  It  is  clear  that  between  cusps,  the 

path  must  be  of  the  form  given  by  Dubins.  Then  they  discard  some  of  the  allowable  curves  by  the 
use  of  a  homotopy  argument. 

Because,  in  this  paper,  we  have  concentrated  on  the  case  in  which  maneuvers  are  allowed,  we 
have  used  these  curves  as  the  basis  for  the  development  of  our  algorithms. 


7.4  Time-Optimal  Paths 

There  has  also  been  some  work  on  planning  time-optimal  trajectories  for  systems  with  constrained 
accelerations.  Fundamental  work  in  trajectory  planning  with  dynamic  constraints  is  presented 
m  [41].  That  paper  introduces  the  idea  of  planning  trajectories  for  a  particle  with  constraints 
on  Its  acceleration.  Unfortunately,  the  analysis  is  restricted  to  the  case  of  one  dimension  motion. 
In  [8]  the  same  problem  is  addressed  in  the  multiple  dimension  case.  That  work  discusses  finding  a 
near-time-optimal  safe  trajectory  for  a  moving  particle  subject  to  uniform  loo  acceleration  bounds 
on  each  axis.  Unfortunately,  the  constraints  on  accelerations  which  are  allowed  in  such  approaches, 
while  they  may  be  state-dependent  [23],  do  not  apply  to  the  systems  we  study  here.  More  recently, 
[9]  presents  the  first  exact  algorithm  for  time-optimal  kinodynamic  motion  planning  in  the  two- 
dimensional  case. 


7.5  Control-Oriented  Approaches 

There  Me  sound  arguments  for  applying  ideas  from  Control  Theory  to  motion  planning.  Control 
theoreticians  have  long  been  interested  in  finding  controllers  which  would  be  robust  to  modeling 
errors,  would  reject  disturbances  and  cope  with  parameter  fiuctuations  (for  example,  see  [13]). 
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There  is  a  small  body  of  literature  on  the  obstacle  avoidance  problem  using  some  ideas  from  Control 
Theory.  Most  of  this  work  centers  on  the  technique  of  potential  fields  introduced  by  Khatib  [25]. 
In  such  problems,  a  potential  function  is  set  up  so  that  the  system  will  travel  to  the  goal  state  by 
moving  in  the  direction  of  the  negative  gradient.  Obstacles  are  avoided  by  making  them  areas  of 
high  potential.  This  work  suffers  from  being  a  local  method  that  can  abort  before  reaching  the 
desired  goal  state.  However,  an  advantage  is  that  these  techniques  are  inherently  robust.  Extensions 
have  been  proposed  by  Rimon  and  Koditschek  [46]  in  an  attempt  to  use  this  approach  for  global 
path  planning.  Krogh  and  Feng  [26]  have  also  proposed  a  method  of  global  planning  in  which  the 
location  of  a  subgoal  is  continuously  changed  to  lead  the  system  around  local  minima  generated  by 
a  set  of  convex  obstacles  to  the  actual  goal. 
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8  Appendix  2  :  The  2-  and  3-Body  Car  Systems 

This  appendix  presents  the  listings  in  Mathematica  of  computations  of  the  various  vector  fields 
corresponding  to  the  examples  of  the  2-  and  3-Body  Systems  respectively. 


(♦ 

♦  A  definition  for  Jacobians  and  Lie  derivatives 
*) 

Clear [Jac,  Lie] 

Jac[x_,  vect_]  :=  Table [D [x [ [i] ] ,  vect[[j]]],  {i.  Length [x] },{ j ,  Length [vect] }] 
Lie[x_,  y_,  vect_]  ;=  Jac[y,  vect] .x  -  Jac[x,  vect].y 
(♦ 

♦  The  2-body  system 
♦) 

vect  =  {x,  y,  theta,  phi} 

(♦  a  simple  lie  function,  that  simplifies  brackets  along  the  way  *) 
«Trigonometry  .m 

slieCx.,  yj  :=  MapCTrigCanonical,  ExpandAll [Lie [x ,  y,  vect]]] 

(*  Phillip  Hall  Family  HI  *) 

xl  =  {Cos  [theta].  Sin  [theta],  0,  -  Sin  [phi]} 

x2  =  {0,  0,  1,  1} 

(*  Phillip  Hall  Family  H2  *) 
x3  *  slie[xl,  x2] 

(♦  Phillip  Hall  Family  H3  *) 
x4  *  slie[xl,  x3] 
x5  =  slie[x2,  x3] 

This  yields  the  following  results: 

rl  =  {cos(O),  sin(^),  0,  —  sin(^)} 
r2  =  {0,0,1,!} 
x3  =  {sin(^),  —  cos(^),  0, 008(9)} 
x4  =  10,0,0,1} 
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det(a:l,x2,x3,®4)  =  1 


(* 

*  The  3-body  system 

*) 

vect  «  {x,  y,  theta,  phil,  phi2} 

(*  Phillip  Hall  Family  HI  *) 

xl  «  {Cos  [theta],  Sin[theta],  0,  -  Sin[phil],  SinCphil]  -  Cos  [phil]  Sin[phi2]} 
x2  »  {0,  0,  1,  1,  0} 

(*  Phillip  Hall  Family  H2  *) 
x3  =  slie[xl,  x2] 

(*  Phillip  Hall  Family  H3  *) 
x4  =  slie[xl,  x3] 
x5  «  slie[x2,  x3] 

(*  Phillip  Hall  Family  H4  *) 
x6  =  slie[xl,  x4] 
x7  =  slie[x2,  x4] 
x8  s  slie[x2,  x5] 

(.*  Phillip  Hall  Family  H5  *) 
x9  -  slie[xl,  x6] 

(♦  an  actual  controllability  algorithm  would  stop  here  ♦) 
xlO  =  slie[x2,  x6] 
xll  =  slie[x2,  x7] 
xl2  s  slie[x2,  x8] 
xl3  s  slie[x3,  x4] 
xl4  =  slie[x3,  x5] 

In  that  case,  we  obtain  the  following  vector  fields. 


xl  =  {cos(^),sin(^),0,  —  sin(y?i),sin(yji)  —  cos(y>i)sin(^2)} 
x2  =  {0,0, 1,1,0} 

x3  =  {8iii(&),  —  cos(^), 0,  cos(^i),  —  cos(v’i)  -  sin(y>i) sin(92)} 
x4  =  {0,0,0, 1,-1 -cos(yj2)} 

x5  =  {co8(ff),  Btn(fi)y  0,  —  8in(yji),  sin(v5i)  —  cos(v>i)  sin(v52)} 
x6  =  {0, 0, 0,  cos(v>i),  —2  cos(y>i)  —  cos(^i)  cos(y>2)} 
x7  =  {0,0, 0,0,0} 

x8  =  {—  sin(d),  cos(tf),  0,  —  cos(v>i),  cos(9Ji)  +  sin(^i)  sin(y>2)} 
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®9  —  {0, 0, 0, 1,  —2  —  2cos(yji)*  cos(y>2)  ~  cos(^2)8iii(9?i)^} 

®10  =  {0,0,0,  — sm(v?i),28in(vJi)  +  cos(y)2)sin(95i)} 

icll  =  {0,0, 0,0,0} 

xl2  =  {-cos(0),-sm(ff),O,sin(<pi),-sin((pi)  +  cos(<pi)sin(<p2)} 
xl3  =  {0,0,0, 6in((^i),— 2sin(v5i)  —  cos(^2)sin(yji)} 
xl4  =  {0,0,0, -1,1  + cos(^2)} 

det(xl,x2,  x3,x4,  x6)  =  —  co8(^i) 
det(xl,x2,x3,x4,x9)  =  -1  -  co8(¥)ifcos(v?2) 
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9  Appendix  3  :  Proof  of  the  Lemma  in  Section  5. 

Note  Xfx]  the  x-coordinate  of  the  vector  field  X. 

Lemma  1  :  The  general  form  of  JC,-,  Y;  and  Z,  is  : 


1-1 


Xi[x] 

— 

8in(«  -  &i) 

i=i 
i— 1 

= 

-  cos(^  - 

i=i 

Xi[0] 

= 

0 

Xi[<po] 

= 

0 

Xi[<Pi-i] 

: 

0 

Xil^i] 

= 

COS  ipi 

= 

—  cos  <fii  —  sin  (fi  sin 

Xi[<fiJr2] 

sin  (^.[sin  y>,+i  -  cos  yjj+i  sin  ipi^2] 

XiWk] 

• 

[si  n  v?fc- 1  —  cos  sin  (/>*]  sin  tpi  H  cos  ipj 

i=t+i 


Yi[x] 

= 

cos{e  -  E 
j=i 

Zi[x] 

= 

-  sin(^  -  E  (fj) 
j=i 

= 

sin(^  -"ttpj) 
j=i 

Zi[y] 

i 

cos(ff  -  E 
j=l 

Ym 

= 

0 

Zi[6] 

= 

0 

Yi[<Po] 

0 

Zil<po] 

=z 

0 

Yi[<fii-i] 

= 

0 

^t[v’t-i] 

zz 

0 

Yi[<Pi] 

= 

0 

Zi[<Pi] 

= 

-1 

Yi[<Pi+i] 

= 

-  sin  (pi+i 

Zil<Pi+i] 

= 

1 

Yi[(pi^.2] 

: 

[sin  (pi+i  -  cos  (fi+i  sin  ^,+2] 

*“2 

Zil<Pi+2] 

i 

0 

YiWk] 

= 

[sin  <pk-i  -  cos  <pk-i  sin  i^k]  FI  cos  (pj 

j=i+l 

Zi[<Pk]  = 

0 

Proof ;  As  in  the  special  case  «  =  2  developed  in  Section  4.6,  we  can  verify  that  the  lemma 
holds  for  »  =  1  (to  do  this,  simply  add  n  -  2  trailers  and  check  the  coordinates  from  tpi  to  ^n)- 
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Assume  now  that  the  lemma  holds  for  A._i,  y;_i  and  Z._i.  We  wiU  just  verify  the  i,  y>.  and  <pk 
coordinates  of  Xi  =  other  computations  being  similar. 

First,  Zi-i  yields  only  four  non-null  coordinates,  of  which  two  are  constant  : 

=  8in(^  -  Zi-i[(pi^i]  =  -1 

i=i 

Zi-\[y]  =  co8(e-£<pj)  Zi.i[<pi]  =  1 

i=i 

Moreover  all  the  partial  derivatives  with  respect  to  x  and  y  vanish.  Therefore  : 


Xi[x] 


Xi[<Pi] 


E  (n-, 


d 


d 


^CO8(0-gy,,) 


f-l 

sin(6i-^V?j) 

i=i 


COS  ipi 


Xi[<pk] 


Q.E.D. 


-  Z,-_  1  [(,Cfc]  [v’i] 

d 

n  V’jOIsin  V’Jt-i  -  cos  sin  (fk] 

j=i 

k-2 

sin  (pi{  n  cos  yjj)[sin  (pk-i  —  cos  (pk-i  sin  <pk] 
j=i+l 
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Computation  of  Holonomic  path:  4.150 


Figure  4:  'I'he  three  steps  of  the  algorit 
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